Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_
00038 #define MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_
00039
00040 #include <string>
00041 #include <vector>
00042 #include <utility>
00043 #include <map>
00044 #include <Eigen/Geometry>
00045 #include <geometric_shapes/shapes.h>
00046 #include <eigen_stl_containers/eigen_stl_vector_container.h>
00047
00048 namespace moveit
00049 {
00050
00051 namespace core
00052 {
00053
00054 class JointModel;
00055 class LinkModel;
00056
00057
00059 typedef std::map<std::string, LinkModel*> LinkModelMap;
00060
00062 typedef std::map<std::string, const LinkModel*> LinkModelMapConst;
00063
00065 typedef std::map<const LinkModel*, Eigen::Affine3d, std::less<const LinkModel*>,
00066 Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > > LinkTransformMap;
00067
00068
00070 class LinkModel
00071 {
00072 public:
00073
00074 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00075
00076 LinkModel(const std::string &name);
00077 ~LinkModel();
00078
00080 const std::string& getName() const
00081 {
00082 return name_;
00083 }
00084
00086 int getLinkIndex() const
00087 {
00088 return link_index_;
00089 }
00090
00091 void setLinkIndex(int index)
00092 {
00093 link_index_ = index;
00094 }
00095
00096 int getFirstCollisionBodyTransformIndex() const
00097 {
00098 return first_collision_body_transform_index_;
00099 }
00100
00101 void setFirstCollisionBodyTransformIndex(int index)
00102 {
00103 first_collision_body_transform_index_ = index;
00104 }
00105
00107 const JointModel* getParentJointModel() const
00108 {
00109 return parent_joint_model_;
00110 }
00111
00112 void setParentJointModel(const JointModel *joint);
00113
00115 const LinkModel* getParentLinkModel() const
00116 {
00117 return parent_link_model_;
00118 }
00119
00120 void setParentLinkModel(const LinkModel *link)
00121 {
00122 parent_link_model_ = link;
00123 }
00124
00126 const std::vector<const JointModel*>& getChildJointModels() const
00127 {
00128 return child_joint_models_;
00129 }
00130
00131 void addChildJointModel(const JointModel *joint)
00132 {
00133 child_joint_models_.push_back(joint);
00134 }
00135
00140 const Eigen::Affine3d& getJointOriginTransform() const
00141 {
00142 return joint_origin_transform_;
00143 }
00144
00145 bool jointOriginTransformIsIdentity() const
00146 {
00147 return joint_origin_transform_is_identity_;
00148 }
00149
00150 bool parentJointIsFixed() const
00151 {
00152 return is_parent_joint_fixed_;
00153 }
00154
00155 void setJointOriginTransform(const Eigen::Affine3d &transform);
00156
00160 const EigenSTL::vector_Affine3d& getCollisionOriginTransforms() const
00161 {
00162 return collision_origin_transform_;
00163 }
00164
00166 const std::vector<int>& areCollisionOriginTransformsIdentity() const
00167 {
00168 return collision_origin_transform_is_identity_;
00169 }
00170
00172 const std::vector<shapes::ShapeConstPtr>& getShapes() const
00173 {
00174 return shapes_;
00175 }
00176
00177 void setGeometry(const std::vector<shapes::ShapeConstPtr> &shapes, const EigenSTL::vector_Affine3d &origins);
00178
00181 const Eigen::Vector3d& getShapeExtentsAtOrigin() const
00182 {
00183 return shape_extents_;
00184 }
00185
00187 const LinkTransformMap& getAssociatedFixedTransforms() const
00188 {
00189 return associated_fixed_transforms_;
00190 }
00191
00193 void addAssociatedFixedTransform(const LinkModel *link_model, const Eigen::Affine3d &transform)
00194 {
00195 associated_fixed_transforms_[link_model] = transform;
00196 }
00197
00199 const std::string& getVisualMeshFilename() const
00200 {
00201 return visual_mesh_filename_;
00202 }
00203
00205 const Eigen::Vector3d& getVisualMeshScale() const
00206 {
00207 return visual_mesh_scale_;
00208 }
00209
00211 const Eigen::Affine3d& getVisualMeshOrigin() const
00212 {
00213 return visual_mesh_origin_;
00214 }
00215
00216 void setVisualMesh(const std::string &visual_mesh, const Eigen::Affine3d &origin, const Eigen::Vector3d &scale);
00217
00218 private:
00219
00221 std::string name_;
00222
00224 const JointModel *parent_joint_model_;
00225
00227 const LinkModel *parent_link_model_;
00228
00230 std::vector<const JointModel*> child_joint_models_;
00231
00233 bool is_parent_joint_fixed_;
00234
00236 bool joint_origin_transform_is_identity_;
00237
00239 Eigen::Affine3d joint_origin_transform_;
00240
00242 EigenSTL::vector_Affine3d collision_origin_transform_;
00243
00245 std::vector<int> collision_origin_transform_is_identity_;
00246
00248 LinkTransformMap associated_fixed_transforms_;
00249
00251 std::vector<shapes::ShapeConstPtr> shapes_;
00252
00254 Eigen::Vector3d shape_extents_;
00255
00257 std::string visual_mesh_filename_;
00258
00260 Eigen::Affine3d visual_mesh_origin_;
00261
00263 Eigen::Vector3d visual_mesh_scale_;
00264
00266 int first_collision_body_transform_index_;
00267
00269 int link_index_;
00270
00271 };
00272 }
00273 }
00274
00275 #endif