Exotica
Public Member Functions | Private Member Functions | Private Attributes | List of all members
exotica::Scene Class Reference

The class of EXOTica Scene. More...

#include <scene.h>

Inheritance diagram for exotica::Scene:
Inheritance graph
Collaboration diagram for exotica::Scene:
Collaboration graph

Public Member Functions

 Scene (const std::string &name)
 
 Scene ()
 
virtual ~Scene ()
 
virtual void Instantiate (const SceneInitializer &init)
 
void RequestKinematics (KinematicsRequest &request, std::function< void(std::shared_ptr< KinematicResponse >)> callback)
 
const std::string & GetName () const
 
void Update (Eigen::VectorXdRefConst x, double t=0)
 
const CollisionScenePtrGetCollisionScene () const
 Returns a pointer to the CollisionScene. More...
 
std::shared_ptr< DynamicsSolverGetDynamicsSolver () const
 Returns a pointer to the CollisionScene. More...
 
std::string GetRootFrameName ()
 
std::string GetRootJointName ()
 
exotica::KinematicTreeGetKinematicTree ()
 
std::vector< std::string > GetControlledJointNames ()
 
std::vector< std::string > GetModelJointNames ()
 
std::vector< std::string > GetControlledLinkNames ()
 
std::vector< std::string > GetModelLinkNames ()
 
Eigen::VectorXd GetControlledState ()
 
Eigen::VectorXd GetModelState ()
 
std::map< std::string, double > GetModelStateMap ()
 
std::map< std::string, std::weak_ptr< KinematicElement > > GetTreeMap ()
 
void SetModelState (Eigen::VectorXdRefConst x, double t=0, bool update_traj=true)
 
void SetModelState (const std::map< std::string, double > &x, double t=0, bool update_traj=true)
 
void AddTrajectoryFromFile (const std::string &link, const std::string &traj)
 
void AddTrajectory (const std::string &link, const std::string &traj)
 
void AddTrajectory (const std::string &link, std::shared_ptr< Trajectory > traj)
 
std::shared_ptr< TrajectoryGetTrajectory (const std::string &link)
 
void RemoveTrajectory (const std::string &link)
 
void UpdateSceneFrames ()
 Updates exotica scene object frames from the MoveIt scene. More...
 
void AttachObject (const std::string &name, const std::string &parent)
 Attaches existing object to a new parent. E.g. attaching a grasping target to the end-effector. The relative transformation will be computed from current object and new parent transformations in the world frame. More...
 
void AttachObjectLocal (const std::string &name, const std::string &parent, const KDL::Frame &pose)
 Attaches existing object to a new parent specifying an offset in the new parent local frame. More...
 
void AttachObjectLocal (const std::string &name, const std::string &parent, const Eigen::VectorXd &pose)
 
void DetachObject (const std::string &name)
 Detaches an object and leaves it a at its current world location. This effectively attaches the object to the world frame. More...
 
bool HasAttachedObject (const std::string &name)
 
void AddObject (const std::string &name, const KDL::Frame &transform=KDL::Frame(), const std::string &parent="", shapes::ShapeConstPtr shape=shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia &inertia=KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d &color=Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene=true)
 
void AddObject (const std::string &name, const KDL::Frame &transform=KDL::Frame(), const std::string &parent="", const std::string &shape_resource_path="", const Eigen::Vector3d &scale=Eigen::Vector3d::Ones(), const KDL::RigidBodyInertia &inertia=KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d &color=Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene=true)
 
void AddObjectToEnvironment (const std::string &name, const KDL::Frame &transform=KDL::Frame(), shapes::ShapeConstPtr shape=nullptr, const Eigen::Vector4d &color=Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene=true)
 
void RemoveObject (const std::string &name)
 
void UpdatePlanningSceneWorld (const moveit_msgs::PlanningSceneWorldConstPtr &world)
 Update the collision scene from a moveit_msgs::PlanningSceneWorld. More...
 
void UpdatePlanningScene (const moveit_msgs::PlanningScene &scene)
 Update the collision scene from a moveit_msgs::PlanningScene. More...
 
moveit_msgs::PlanningScene GetPlanningSceneMsg ()
 Returns the current robot configuration and collision environment as a moveit_msgs::PlanningScene. More...
 
void UpdateCollisionObjects ()
 
void UpdateTrajectoryGenerators (double t=0)
 
void PublishScene ()
 
void PublishProxies (const std::vector< CollisionProxy > &proxies)
 
visualization_msgs::Marker ProxyToMarker (const std::vector< CollisionProxy > &proxies, const std::string &frame)
 
void LoadScene (const std::string &scene, const Eigen::Isometry3d &offset=Eigen::Isometry3d::Identity(), bool update_collision_scene=true)
 
void LoadScene (const std::string &scene, const KDL::Frame &offset=KDL::Frame(), bool update_collision_scene=true)
 
void LoadSceneFile (const std::string &file_name, const Eigen::Isometry3d &offset=Eigen::Isometry3d::Identity(), bool update_collision_scene=true)
 
void LoadSceneFile (const std::string &file_name, const KDL::Frame &offset=KDL::Frame(), bool update_collision_scene=true)
 
std::string GetScene ()
 
void CleanScene ()
 
bool AlwaysUpdatesCollisionScene () const
 Whether the collision scene transforms get updated on every scene update. More...
 
const std::map< std::string, std::vector< std::string > > & GetModelLinkToCollisionLinkMap () const
 Returns a map between a model link name and the names of associated collision links. More...
 
const std::map< std::string, std::vector< std::shared_ptr< KinematicElement > > > & GetModelLinkToCollisionElementMap () const
 Returns a map between a model link name and the KinematicElement of associated collision links. More...
 
const std::map< std::string, std::vector< std::string > > & GetControlledJointToCollisionLinkMap () const
 Returns a map between controlled robot joint names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled joint (as if the collision links had been fused). More...
 
const std::set< std::string > & get_world_links_to_exclude_from_collision_scene () const
 Returns world links that are to be excluded from collision checking. More...
 
int get_num_positions () const
 
int get_num_velocities () const
 
int get_num_controls () const
 
int get_num_state () const
 
int get_num_state_derivative () const
 
bool get_has_quaternion_floating_base () const
 
- Public Member Functions inherited from exotica::Object
 Object ()
 
virtual ~Object ()
 
virtual std::string type () const
 Type Information wrapper: must be virtual so that it is polymorphic... More...
 
std::string GetObjectName ()
 
void InstantiateObject (const Initializer &init)
 
virtual std::string Print (const std::string &prepend) const
 
- Public Member Functions inherited from exotica::Instantiable< SceneInitializer >
void InstantiateInternal (const Initializer &init) override
 
Initializer GetInitializerTemplate () override
 
std::vector< InitializerGetAllTemplates () const override
 
const SceneInitializer & GetParameters () const
 
- Public Member Functions inherited from exotica::InstantiableBase
 InstantiableBase ()=default
 
virtual ~InstantiableBase ()=default
 
virtual void InstantiateBase (const Initializer &init)
 
virtual std::vector< InitializerGetAllTemplates () const =0
 

Private Member Functions

void UpdateInternalFrames (bool update_request=true)
 
void UpdateMoveItPlanningScene ()
 Updates the internal state of the MoveIt PlanningScene from Kinematica. More...
 
void LoadSceneFromStringStream (std::istream &in, const Eigen::Isometry3d &offset, bool update_collision_scene)
 
- Private Member Functions inherited from exotica::Uncopyable
 Uncopyable ()=default
 
 ~Uncopyable ()=default
 

Private Attributes

bool has_quaternion_floating_base_ = false
 Whether the state includes a SE(3) floating base. More...
 
exotica::KinematicTree kinematica_
 The kinematica tree. More...
 
CollisionScenePtr collision_scene_
 The collision scene. More...
 
std::shared_ptr< DynamicsSolverdynamics_solver_ = std::shared_ptr<DynamicsSolver>(nullptr)
 The dynamics solver. More...
 
int num_positions_ = 0
 "nq" More...
 
int num_velocities_ = 0
 "nv" More...
 
int num_controls_ = 0
 "nu" More...
 
int num_state_ = 0
 "nx" More...
 
int num_state_derivative_ = 0
 "ndx" More...
 
planning_scene::PlanningScenePtr ps_
 Internal MoveIt planning scene. More...
 
ros::Publisher ps_pub_
 Visual debug. More...
 
ros::Publisher proxy_pub_
 
std::map< std::string, AttachedObjectattached_objects_
 List of attached objects These objects will be reattached if the scene gets reloaded. More...
 
std::vector< std::shared_ptr< KinematicElement > > custom_links_
 List of frames/links added on top of robot links and scene objects defined in the MoveIt scene. More...
 
std::map< std::string, std::pair< std::weak_ptr< KinematicElement >, std::shared_ptr< Trajectory > > > trajectory_generators_
 
bool force_collision_
 
std::map< std::string, std::vector< std::string > > model_link_to_collision_link_map_
 Mapping between model link names and collision links. More...
 
std::map< std::string, std::vector< std::shared_ptr< KinematicElement > > > model_link_to_collision_element_map_
 
std::map< std::string, std::vector< std::string > > controlled_joint_to_collision_link_map_
 Mapping between controlled joint name and collision links. More...
 
std::set< std::string > robot_links_to_exclude_from_collision_scene_
 List of robot links to be excluded from the collision scene. More...
 
std::set< std::string > world_links_to_exclude_from_collision_scene_
 List of world links to be excluded from the collision scene. More...
 
KinematicsRequest kinematic_request_
 
std::shared_ptr< KinematicResponsekinematic_solution_
 
std::function< void(std::shared_ptr< KinematicResponse >)> kinematic_request_callback_
 
bool request_needs_updating_
 

Additional Inherited Members

- Public Attributes inherited from exotica::Object
std::string ns_
 
std::string object_name_
 
bool debug_
 
- Protected Attributes inherited from exotica::Instantiable< SceneInitializer >
SceneInitializer parameters_
 

Detailed Description

The class of EXOTica Scene.

Constructor & Destructor Documentation

◆ Scene() [1/2]

exotica::Scene::Scene ( const std::string &  name)

◆ Scene() [2/2]

exotica::Scene::Scene ( )

◆ ~Scene()

virtual exotica::Scene::~Scene ( )
virtual

Member Function Documentation

◆ AddObject() [1/2]

void exotica::Scene::AddObject ( const std::string &  name,
const KDL::Frame &  transform = KDL::Frame(),
const std::string &  parent = "",
const std::string &  shape_resource_path = "",
const Eigen::Vector3d &  scale = Eigen::Vector3d::Ones(),
const KDL::RigidBodyInertia &  inertia = KDL::RigidBodyInertia::Zero(),
const Eigen::Vector4d &  color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
const bool  update_collision_scene = true 
)

◆ AddObject() [2/2]

void exotica::Scene::AddObject ( const std::string &  name,
const KDL::Frame &  transform = KDL::Frame(),
const std::string &  parent = "",
shapes::ShapeConstPtr  shape = shapes::ShapeConstPtr(nullptr),
const KDL::RigidBodyInertia &  inertia = KDL::RigidBodyInertia::Zero(),
const Eigen::Vector4d &  color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
const bool  update_collision_scene = true 
)

◆ AddObjectToEnvironment()

void exotica::Scene::AddObjectToEnvironment ( const std::string &  name,
const KDL::Frame &  transform = KDL::Frame(),
shapes::ShapeConstPtr  shape = nullptr,
const Eigen::Vector4d &  color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0),
const bool  update_collision_scene = true 
)

◆ AddTrajectory() [1/2]

void exotica::Scene::AddTrajectory ( const std::string &  link,
const std::string &  traj 
)

◆ AddTrajectory() [2/2]

void exotica::Scene::AddTrajectory ( const std::string &  link,
std::shared_ptr< Trajectory traj 
)

◆ AddTrajectoryFromFile()

void exotica::Scene::AddTrajectoryFromFile ( const std::string &  link,
const std::string &  traj 
)

◆ AlwaysUpdatesCollisionScene()

bool exotica::Scene::AlwaysUpdatesCollisionScene ( ) const
inline

Whether the collision scene transforms get updated on every scene update.

Returns
Whether collision scene transforms are force updated on every scene update.

◆ AttachObject()

void exotica::Scene::AttachObject ( const std::string &  name,
const std::string &  parent 
)

Attaches existing object to a new parent. E.g. attaching a grasping target to the end-effector. The relative transformation will be computed from current object and new parent transformations in the world frame.

Parameters
nameName of the object to attach.
parentName of the new parent frame.

◆ AttachObjectLocal() [1/2]

void exotica::Scene::AttachObjectLocal ( const std::string &  name,
const std::string &  parent,
const Eigen::VectorXd &  pose 
)

◆ AttachObjectLocal() [2/2]

void exotica::Scene::AttachObjectLocal ( const std::string &  name,
const std::string &  parent,
const KDL::Frame &  pose 
)

Attaches existing object to a new parent specifying an offset in the new parent local frame.

Parameters
nameName of the object to attach.
parentName of the new parent frame.
poseRelative pose of the attached object in the new parent's local frame.

◆ CleanScene()

void exotica::Scene::CleanScene ( )

◆ DetachObject()

void exotica::Scene::DetachObject ( const std::string &  name)

Detaches an object and leaves it a at its current world location. This effectively attaches the object to the world frame.

Parameters
nameName of the object to detach.

◆ get_has_quaternion_floating_base()

bool exotica::Scene::get_has_quaternion_floating_base ( ) const

◆ get_num_controls()

int exotica::Scene::get_num_controls ( ) const

◆ get_num_positions()

int exotica::Scene::get_num_positions ( ) const

◆ get_num_state()

int exotica::Scene::get_num_state ( ) const

◆ get_num_state_derivative()

int exotica::Scene::get_num_state_derivative ( ) const

◆ get_num_velocities()

int exotica::Scene::get_num_velocities ( ) const

◆ get_world_links_to_exclude_from_collision_scene()

const std::set<std::string>& exotica::Scene::get_world_links_to_exclude_from_collision_scene ( ) const
inline

Returns world links that are to be excluded from collision checking.

◆ GetCollisionScene()

const CollisionScenePtr& exotica::Scene::GetCollisionScene ( ) const

Returns a pointer to the CollisionScene.

◆ GetControlledJointNames()

std::vector<std::string> exotica::Scene::GetControlledJointNames ( )

◆ GetControlledJointToCollisionLinkMap()

const std::map<std::string, std::vector<std::string> >& exotica::Scene::GetControlledJointToCollisionLinkMap ( ) const
inline

Returns a map between controlled robot joint names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled joint (as if the collision links had been fused).

Returns
Map between controlled joints and associated collision links.

◆ GetControlledLinkNames()

std::vector<std::string> exotica::Scene::GetControlledLinkNames ( )

◆ GetControlledState()

Eigen::VectorXd exotica::Scene::GetControlledState ( )

◆ GetDynamicsSolver()

std::shared_ptr<DynamicsSolver> exotica::Scene::GetDynamicsSolver ( ) const

Returns a pointer to the CollisionScene.

◆ GetKinematicTree()

exotica::KinematicTree& exotica::Scene::GetKinematicTree ( )

◆ GetModelJointNames()

std::vector<std::string> exotica::Scene::GetModelJointNames ( )

◆ GetModelLinkNames()

std::vector<std::string> exotica::Scene::GetModelLinkNames ( )

◆ GetModelLinkToCollisionElementMap()

const std::map<std::string, std::vector<std::shared_ptr<KinematicElement> > >& exotica::Scene::GetModelLinkToCollisionElementMap ( ) const
inline

Returns a map between a model link name and the KinematicElement of associated collision links.

Returns
Map between model links and all the KinematicElements of the associated collision links.

◆ GetModelLinkToCollisionLinkMap()

const std::map<std::string, std::vector<std::string> >& exotica::Scene::GetModelLinkToCollisionLinkMap ( ) const
inline

Returns a map between a model link name and the names of associated collision links.

Returns
Map between model links and all associated collision links.

◆ GetModelState()

Eigen::VectorXd exotica::Scene::GetModelState ( )

◆ GetModelStateMap()

std::map<std::string, double> exotica::Scene::GetModelStateMap ( )

◆ GetName()

const std::string& exotica::Scene::GetName ( ) const

◆ GetPlanningSceneMsg()

moveit_msgs::PlanningScene exotica::Scene::GetPlanningSceneMsg ( )

Returns the current robot configuration and collision environment as a moveit_msgs::PlanningScene.

◆ GetRootFrameName()

std::string exotica::Scene::GetRootFrameName ( )

◆ GetRootJointName()

std::string exotica::Scene::GetRootJointName ( )

◆ GetScene()

std::string exotica::Scene::GetScene ( )

◆ GetTrajectory()

std::shared_ptr<Trajectory> exotica::Scene::GetTrajectory ( const std::string &  link)

◆ GetTreeMap()

std::map<std::string, std::weak_ptr<KinematicElement> > exotica::Scene::GetTreeMap ( )

◆ HasAttachedObject()

bool exotica::Scene::HasAttachedObject ( const std::string &  name)

◆ Instantiate()

virtual void exotica::Scene::Instantiate ( const SceneInitializer &  init)
virtual

◆ LoadScene() [1/2]

void exotica::Scene::LoadScene ( const std::string &  scene,
const Eigen::Isometry3d &  offset = Eigen::Isometry3d::Identity(),
bool  update_collision_scene = true 
)

◆ LoadScene() [2/2]

void exotica::Scene::LoadScene ( const std::string &  scene,
const KDL::Frame &  offset = KDL::Frame(),
bool  update_collision_scene = true 
)

◆ LoadSceneFile() [1/2]

void exotica::Scene::LoadSceneFile ( const std::string &  file_name,
const Eigen::Isometry3d &  offset = Eigen::Isometry3d::Identity(),
bool  update_collision_scene = true 
)

◆ LoadSceneFile() [2/2]

void exotica::Scene::LoadSceneFile ( const std::string &  file_name,
const KDL::Frame &  offset = KDL::Frame(),
bool  update_collision_scene = true 
)

◆ LoadSceneFromStringStream()

void exotica::Scene::LoadSceneFromStringStream ( std::istream &  in,
const Eigen::Isometry3d &  offset,
bool  update_collision_scene 
)
private

◆ ProxyToMarker()

visualization_msgs::Marker exotica::Scene::ProxyToMarker ( const std::vector< CollisionProxy > &  proxies,
const std::string &  frame 
)

◆ PublishProxies()

void exotica::Scene::PublishProxies ( const std::vector< CollisionProxy > &  proxies)

◆ PublishScene()

void exotica::Scene::PublishScene ( )

◆ RemoveObject()

void exotica::Scene::RemoveObject ( const std::string &  name)

◆ RemoveTrajectory()

void exotica::Scene::RemoveTrajectory ( const std::string &  link)

◆ RequestKinematics()

void exotica::Scene::RequestKinematics ( KinematicsRequest request,
std::function< void(std::shared_ptr< KinematicResponse >)>  callback 
)

◆ SetModelState() [1/2]

void exotica::Scene::SetModelState ( const std::map< std::string, double > &  x,
double  t = 0,
bool  update_traj = true 
)

◆ SetModelState() [2/2]

void exotica::Scene::SetModelState ( Eigen::VectorXdRefConst  x,
double  t = 0,
bool  update_traj = true 
)

◆ Update()

void exotica::Scene::Update ( Eigen::VectorXdRefConst  x,
double  t = 0 
)

◆ UpdateCollisionObjects()

void exotica::Scene::UpdateCollisionObjects ( )

◆ UpdateInternalFrames()

void exotica::Scene::UpdateInternalFrames ( bool  update_request = true)
private

◆ UpdateMoveItPlanningScene()

void exotica::Scene::UpdateMoveItPlanningScene ( )
private

Updates the internal state of the MoveIt PlanningScene from Kinematica.

◆ UpdatePlanningScene()

void exotica::Scene::UpdatePlanningScene ( const moveit_msgs::PlanningScene &  scene)

Update the collision scene from a moveit_msgs::PlanningScene.

Parameters
[in]scenemoveit_msgs::PlanningScene

◆ UpdatePlanningSceneWorld()

void exotica::Scene::UpdatePlanningSceneWorld ( const moveit_msgs::PlanningSceneWorldConstPtr &  world)

Update the collision scene from a moveit_msgs::PlanningSceneWorld.

Parameters
[in]worldmoveit_msgs::PlanningSceneWorld

◆ UpdateSceneFrames()

void exotica::Scene::UpdateSceneFrames ( )

Updates exotica scene object frames from the MoveIt scene.

◆ UpdateTrajectoryGenerators()

void exotica::Scene::UpdateTrajectoryGenerators ( double  t = 0)

Member Data Documentation

◆ attached_objects_

std::map<std::string, AttachedObject> exotica::Scene::attached_objects_
private

List of attached objects These objects will be reattached if the scene gets reloaded.

◆ collision_scene_

CollisionScenePtr exotica::Scene::collision_scene_
private

The collision scene.

◆ controlled_joint_to_collision_link_map_

std::map<std::string, std::vector<std::string> > exotica::Scene::controlled_joint_to_collision_link_map_
private

Mapping between controlled joint name and collision links.

◆ custom_links_

std::vector<std::shared_ptr<KinematicElement> > exotica::Scene::custom_links_
private

List of frames/links added on top of robot links and scene objects defined in the MoveIt scene.

◆ dynamics_solver_

std::shared_ptr<DynamicsSolver> exotica::Scene::dynamics_solver_ = std::shared_ptr<DynamicsSolver>(nullptr)
private

The dynamics solver.

◆ force_collision_

bool exotica::Scene::force_collision_
private

◆ has_quaternion_floating_base_

bool exotica::Scene::has_quaternion_floating_base_ = false
private

Whether the state includes a SE(3) floating base.

◆ kinematic_request_

KinematicsRequest exotica::Scene::kinematic_request_
private

◆ kinematic_request_callback_

std::function<void(std::shared_ptr<KinematicResponse>)> exotica::Scene::kinematic_request_callback_
private

◆ kinematic_solution_

std::shared_ptr<KinematicResponse> exotica::Scene::kinematic_solution_
private

◆ kinematica_

exotica::KinematicTree exotica::Scene::kinematica_
private

The kinematica tree.

◆ model_link_to_collision_element_map_

std::map<std::string, std::vector<std::shared_ptr<KinematicElement> > > exotica::Scene::model_link_to_collision_element_map_
private

◆ model_link_to_collision_link_map_

std::map<std::string, std::vector<std::string> > exotica::Scene::model_link_to_collision_link_map_
private

Mapping between model link names and collision links.

◆ num_controls_

int exotica::Scene::num_controls_ = 0
private

"nu"

◆ num_positions_

int exotica::Scene::num_positions_ = 0
private

"nq"

◆ num_state_

int exotica::Scene::num_state_ = 0
private

"nx"

◆ num_state_derivative_

int exotica::Scene::num_state_derivative_ = 0
private

"ndx"

◆ num_velocities_

int exotica::Scene::num_velocities_ = 0
private

"nv"

◆ proxy_pub_

ros::Publisher exotica::Scene::proxy_pub_
private

◆ ps_

planning_scene::PlanningScenePtr exotica::Scene::ps_
private

Internal MoveIt planning scene.

◆ ps_pub_

ros::Publisher exotica::Scene::ps_pub_
private

Visual debug.

◆ request_needs_updating_

bool exotica::Scene::request_needs_updating_
private

◆ robot_links_to_exclude_from_collision_scene_

std::set<std::string> exotica::Scene::robot_links_to_exclude_from_collision_scene_
private

List of robot links to be excluded from the collision scene.

◆ trajectory_generators_

std::map<std::string, std::pair<std::weak_ptr<KinematicElement>, std::shared_ptr<Trajectory> > > exotica::Scene::trajectory_generators_
private

◆ world_links_to_exclude_from_collision_scene_

std::set<std::string> exotica::Scene::world_links_to_exclude_from_collision_scene_
private

List of world links to be excluded from the collision scene.


The documentation for this class was generated from the following file: