37 #ifndef MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ 38 #define MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_ 44 #include <Eigen/Geometry> 67 typedef std::map<const LinkModel*, Eigen::Isometry3d, std::less<const LinkModel*>,
68 Eigen::aligned_allocator<std::pair<const LinkModel* const, Eigen::Isometry3d> > >
99 return first_collision_body_transform_index_;
104 first_collision_body_transform_index_ = index;
110 return parent_joint_model_;
113 void setParentJointModel(
const JointModel* joint);
119 return parent_link_model_;
124 parent_link_model_ = link;
130 return child_joint_models_;
135 child_joint_models_.push_back(joint);
144 return joint_origin_transform_;
149 return joint_origin_transform_is_identity_;
154 return is_parent_joint_fixed_;
157 void setJointOriginTransform(
const Eigen::Isometry3d& transform);
164 return collision_origin_transform_;
170 return collision_origin_transform_is_identity_;
174 const std::vector<shapes::ShapeConstPtr>&
getShapes()
const 186 return shape_extents_;
192 return centered_bounding_box_offset_;
198 return associated_fixed_transforms_;
204 associated_fixed_transforms_[link_model] = transform;
210 return visual_mesh_filename_;
216 return visual_mesh_scale_;
222 return visual_mesh_origin_;
225 void setVisualMesh(
const std::string& visual_mesh,
const Eigen::Isometry3d& origin,
const Eigen::Vector3d& scale);
std::vector< const JointModel * > child_joint_models_
List of directly descending joints (each connects to a child link)
std::map< std::string, const LinkModel * > LinkModelMapConst
Map of names to const instances for LinkModel.
const Eigen::Isometry3d & getVisualMeshOrigin() const
Get the transform for the visual mesh origin.
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
int first_collision_body_transform_index_
Index of the transform for the first shape that makes up the geometry of this link in the full robot ...
std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > LinkTransformMap
Map from link model instances to Eigen transforms.
const Eigen::Isometry3d & getJointOriginTransform() const
When transforms are computed for this link, they are usually applied to the link's origin...
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link's geometry (dimensions of axis-aligned bounding box around all shapes tha...
Vec3fX< details::Vec3Data< double > > Vector3d
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Eigen::Isometry3d joint_origin_transform_
The constant transform applied to the link (local)
LinkTransformMap associated_fixed_transforms_
The set of links that are attached to this one via fixed transforms.
const std::string & getVisualMeshFilename() const
Get the filename of the mesh resource used for visual display of this link.
bool parentJointIsFixed() const
const LinkTransformMap & getAssociatedFixedTransforms() const
Get the set of links that are attached to this one via fixed transforms.
Eigen::Vector3d visual_mesh_scale_
Scale factor associated with the visual geometry mesh of this link.
const Eigen::Vector3d & getVisualMeshScale() const
Get the scale of the mesh resource for this link.
void setFirstCollisionBodyTransformIndex(int index)
void addChildJointModel(const JointModel *joint)
const JointModel * parent_joint_model_
JointModel that connects this link to the parent link.
std::vector< int > collision_origin_transform_is_identity_
Flag indicating if the constant transform applied to the collision geometry of the link (local) is id...
MOVEIT_CLASS_FORWARD(Shape)
const LinkModel * getParentLinkModel() const
Get the link model whose child this link is (through some joint). There may not always be a parent li...
Eigen::Isometry3d visual_mesh_origin_
The additional origin transform for the mesh.
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
void addAssociatedFixedTransform(const LinkModel *link_model, const Eigen::Isometry3d &transform)
Remember that link_model is attached to this link using a fixed transform.
const std::vector< int > & areCollisionOriginTransformsIdentity() const
Return flags for each transform specifying whether they are identity or not.
const std::string & getName() const
The name of this link.
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
void setLinkIndex(int index)
Eigen::Vector3d shape_extents_
The extents of shape (dimensions of axis aligned bounding box when shape is at origin).
void setParentLinkModel(const LinkModel *link)
int getFirstCollisionBodyTransformIndex() const
bool joint_origin_transform_is_identity_
True of the joint origin transform is identity.
const std::vector< const JointModel * > & getChildJointModels() const
A link may have 0 or more child joints. From those joints there will certainly be other descendant li...
std::map< std::string, LinkModel * > LinkModelMap
Map of names to instances for LinkModel.
const EigenSTL::vector_Isometry3d & getCollisionOriginTransforms() const
In addition to the link transform, the geometry of a link that is used for collision checking may hav...
EigenSTL::vector_Isometry3d collision_origin_transform_
The constant transform applied to the collision geometry of the link (local)
Eigen::Vector3d centered_bounding_box_offset_
Center of the axis aligned bounding box with size shape_extents_ (zero if symmetric along all axes)...
std::string visual_mesh_filename_
Filename associated with the visual geometry mesh of this link. If empty, no mesh was used...
int link_index_
Index of the transform for this link in the full robot frame.
A link from the robot. Contains the constant transform applied to the link and its geometry...
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
const LinkModel * parent_link_model_
The parent link model (NULL for the root link)
bool jointOriginTransformIsIdentity() const
Main namespace for MoveIt!
std::vector< shapes::ShapeConstPtr > shapes_
The collision geometry of the link.
std::string name_
Name of the link.
const Eigen::Vector3d & getCenteredBoundingBoxOffset() const
Get the offset of the center of the bounding box of this link when the link is positioned at origin...
bool is_parent_joint_fixed_
True if the parent joint of this link is fixed.