Exotica
|
The class of EXOTica Scene. More...
#include <scene.h>
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 CollisionScenePtr & | GetCollisionScene () const |
Returns a pointer to the CollisionScene. More... | |
std::shared_ptr< DynamicsSolver > | GetDynamicsSolver () const |
Returns a pointer to the CollisionScene. More... | |
std::string | GetRootFrameName () |
std::string | GetRootJointName () |
exotica::KinematicTree & | GetKinematicTree () |
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< Trajectory > | GetTrajectory (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< Initializer > | GetAllTemplates () 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< Initializer > | GetAllTemplates () 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< DynamicsSolver > | dynamics_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, AttachedObject > | attached_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< KinematicResponse > | kinematic_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_ |
The class of EXOTica Scene.
exotica::Scene::Scene | ( | const std::string & | name | ) |
exotica::Scene::Scene | ( | ) |
|
virtual |
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 |
||
) |
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 |
||
) |
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 |
||
) |
void exotica::Scene::AddTrajectory | ( | const std::string & | link, |
const std::string & | traj | ||
) |
void exotica::Scene::AddTrajectory | ( | const std::string & | link, |
std::shared_ptr< Trajectory > | traj | ||
) |
void exotica::Scene::AddTrajectoryFromFile | ( | const std::string & | link, |
const std::string & | traj | ||
) |
|
inline |
Whether the collision scene transforms get updated on every scene update.
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.
name | Name of the object to attach. |
parent | Name of the new parent frame. |
void exotica::Scene::AttachObjectLocal | ( | const std::string & | name, |
const std::string & | parent, | ||
const Eigen::VectorXd & | pose | ||
) |
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.
name | Name of the object to attach. |
parent | Name of the new parent frame. |
pose | Relative pose of the attached object in the new parent's local frame. |
void exotica::Scene::CleanScene | ( | ) |
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.
name | Name of the object to detach. |
bool exotica::Scene::get_has_quaternion_floating_base | ( | ) | const |
int exotica::Scene::get_num_controls | ( | ) | const |
int exotica::Scene::get_num_positions | ( | ) | const |
int exotica::Scene::get_num_state | ( | ) | const |
int exotica::Scene::get_num_state_derivative | ( | ) | const |
int exotica::Scene::get_num_velocities | ( | ) | const |
|
inline |
Returns world links that are to be excluded from collision checking.
const CollisionScenePtr& exotica::Scene::GetCollisionScene | ( | ) | const |
Returns a pointer to the CollisionScene.
std::vector<std::string> exotica::Scene::GetControlledJointNames | ( | ) |
|
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).
std::vector<std::string> exotica::Scene::GetControlledLinkNames | ( | ) |
Eigen::VectorXd exotica::Scene::GetControlledState | ( | ) |
std::shared_ptr<DynamicsSolver> exotica::Scene::GetDynamicsSolver | ( | ) | const |
Returns a pointer to the CollisionScene.
exotica::KinematicTree& exotica::Scene::GetKinematicTree | ( | ) |
std::vector<std::string> exotica::Scene::GetModelJointNames | ( | ) |
std::vector<std::string> exotica::Scene::GetModelLinkNames | ( | ) |
|
inline |
Returns a map between a model link name and the KinematicElement of associated collision links.
|
inline |
Returns a map between a model link name and the names of associated collision links.
Eigen::VectorXd exotica::Scene::GetModelState | ( | ) |
std::map<std::string, double> exotica::Scene::GetModelStateMap | ( | ) |
const std::string& exotica::Scene::GetName | ( | ) | const |
moveit_msgs::PlanningScene exotica::Scene::GetPlanningSceneMsg | ( | ) |
Returns the current robot configuration and collision environment as a moveit_msgs::PlanningScene.
std::string exotica::Scene::GetRootFrameName | ( | ) |
std::string exotica::Scene::GetRootJointName | ( | ) |
std::string exotica::Scene::GetScene | ( | ) |
std::shared_ptr<Trajectory> exotica::Scene::GetTrajectory | ( | const std::string & | link | ) |
std::map<std::string, std::weak_ptr<KinematicElement> > exotica::Scene::GetTreeMap | ( | ) |
bool exotica::Scene::HasAttachedObject | ( | const std::string & | name | ) |
|
virtual |
Reimplemented from exotica::Instantiable< SceneInitializer >.
void exotica::Scene::LoadScene | ( | const std::string & | scene, |
const Eigen::Isometry3d & | offset = Eigen::Isometry3d::Identity() , |
||
bool | update_collision_scene = true |
||
) |
void exotica::Scene::LoadScene | ( | const std::string & | scene, |
const KDL::Frame & | offset = KDL::Frame() , |
||
bool | update_collision_scene = true |
||
) |
void exotica::Scene::LoadSceneFile | ( | const std::string & | file_name, |
const Eigen::Isometry3d & | offset = Eigen::Isometry3d::Identity() , |
||
bool | update_collision_scene = true |
||
) |
void exotica::Scene::LoadSceneFile | ( | const std::string & | file_name, |
const KDL::Frame & | offset = KDL::Frame() , |
||
bool | update_collision_scene = true |
||
) |
|
private |
visualization_msgs::Marker exotica::Scene::ProxyToMarker | ( | const std::vector< CollisionProxy > & | proxies, |
const std::string & | frame | ||
) |
void exotica::Scene::PublishProxies | ( | const std::vector< CollisionProxy > & | proxies | ) |
void exotica::Scene::PublishScene | ( | ) |
void exotica::Scene::RemoveObject | ( | const std::string & | name | ) |
void exotica::Scene::RemoveTrajectory | ( | const std::string & | link | ) |
void exotica::Scene::RequestKinematics | ( | KinematicsRequest & | request, |
std::function< void(std::shared_ptr< KinematicResponse >)> | callback | ||
) |
void exotica::Scene::SetModelState | ( | const std::map< std::string, double > & | x, |
double | t = 0 , |
||
bool | update_traj = true |
||
) |
void exotica::Scene::SetModelState | ( | Eigen::VectorXdRefConst | x, |
double | t = 0 , |
||
bool | update_traj = true |
||
) |
void exotica::Scene::Update | ( | Eigen::VectorXdRefConst | x, |
double | t = 0 |
||
) |
void exotica::Scene::UpdateCollisionObjects | ( | ) |
|
private |
|
private |
Updates the internal state of the MoveIt PlanningScene from Kinematica.
void exotica::Scene::UpdatePlanningScene | ( | const moveit_msgs::PlanningScene & | scene | ) |
Update the collision scene from a moveit_msgs::PlanningScene.
[in] | scene | moveit_msgs::PlanningScene |
void exotica::Scene::UpdatePlanningSceneWorld | ( | const moveit_msgs::PlanningSceneWorldConstPtr & | world | ) |
Update the collision scene from a moveit_msgs::PlanningSceneWorld.
[in] | world | moveit_msgs::PlanningSceneWorld |
void exotica::Scene::UpdateSceneFrames | ( | ) |
Updates exotica scene object frames from the MoveIt scene.
void exotica::Scene::UpdateTrajectoryGenerators | ( | double | t = 0 | ) |
|
private |
List of attached objects These objects will be reattached if the scene gets reloaded.
|
private |
The collision scene.
|
private |
Mapping between controlled joint name and collision links.
|
private |
List of frames/links added on top of robot links and scene objects defined in the MoveIt scene.
|
private |
The dynamics solver.
|
private |
|
private |
Whether the state includes a SE(3) floating base.
|
private |
|
private |
|
private |
|
private |
The kinematica tree.
|
private |
|
private |
Mapping between model link names and collision links.
|
private |
"nu"
|
private |
"nq"
|
private |
"nx"
|
private |
"ndx"
|
private |
"nv"
|
private |
|
private |
Internal MoveIt planning scene.
|
private |
Visual debug.
|
private |
|
private |
List of robot links to be excluded from the collision scene.
|
private |
|
private |
List of world links to be excluded from the collision scene.