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_ROBOT_MODEL_ROBOT_MODEL_
00038 #define MOVEIT_ROBOT_MODEL_ROBOT_MODEL_
00039
00040 #include <urdf_model/model.h>
00041 #include <srdfdom/model.h>
00042 #include <geometric_shapes/shapes.h>
00043 #include <geometric_shapes/shape_messages.h>
00044 #include <random_numbers/random_numbers.h>
00045 #include <moveit/robot_model/joint_model_group.h>
00046 #include <moveit/robot_model/fixed_joint_model.h>
00047 #include <moveit/robot_model/floating_joint_model.h>
00048 #include <moveit/robot_model/planar_joint_model.h>
00049 #include <moveit/robot_model/revolute_joint_model.h>
00050 #include <moveit/robot_model/prismatic_joint_model.h>
00051 #include <boost/shared_ptr.hpp>
00052 #include <iostream>
00053 #include <set>
00054
00055 #include <Eigen/Geometry>
00056
00057 #include <console_bridge/console.h>
00058
00060 namespace robot_model
00061 {
00062
00065 class RobotModel
00066 {
00067 public:
00068
00070 RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00071 const boost::shared_ptr<const srdf::Model> &srdf_model);
00072
00074 virtual ~RobotModel();
00075
00077 const std::string& getName() const
00078 {
00079 return model_name_;
00080 }
00081
00083 const boost::shared_ptr<const urdf::ModelInterface>& getURDF() const
00084 {
00085 return urdf_;
00086 }
00087
00089 const boost::shared_ptr<const srdf::Model>& getSRDF() const
00090 {
00091 return srdf_;
00092 }
00093
00095 bool hasLinkModel(const std::string &name) const;
00096
00098 const LinkModel* getLinkModel(const std::string &link) const;
00099
00101 bool hasJointModel(const std::string &name) const;
00102
00104 const JointModel* getJointModel(const std::string &joint) const;
00105
00107 void getChildLinkModels(const LinkModel* parent, std::vector<const LinkModel*> &links) const;
00108
00110 void getChildLinkModels(const JointModel* parent, std::vector<const LinkModel*> &links) const;
00111
00113 void getChildJointModels(const LinkModel* parent, std::vector<const JointModel*> &links) const;
00114
00116 void getChildJointModels(const JointModel* parent, std::vector<const JointModel*> &links) const;
00117
00119 std::vector<std::string> getChildLinkModelNames(const LinkModel* parent) const;
00120
00122 std::vector<std::string> getChildJointModelNames(const LinkModel* parent) const;
00123
00125 std::vector<std::string> getChildJointModelNames(const JointModel* parent) const;
00126
00128 std::vector<std::string> getChildLinkModelNames(const JointModel* parent) const;
00129
00132 const std::vector<const JointModel*>& getJointModels() const
00133 {
00134 return joint_model_vector_const_;
00135 }
00136
00139 const std::vector<JointModel*>& getJointModels()
00140 {
00141 return joint_model_vector_;
00142 }
00143
00145 const std::vector<std::string>& getJointModelNames() const
00146 {
00147 return joint_model_names_vector_;
00148 }
00149
00152 const std::vector<const JointModel*>& getContinuousJointModels() const
00153 {
00154 return continuous_joint_model_vector_const_;
00155 }
00156
00158 const std::vector<const LinkModel*>& getLinkModels() const
00159 {
00160 return link_model_vector_const_;
00161 }
00162
00164 const std::vector<LinkModel*>& getLinkModels()
00165 {
00166 return link_model_vector_;
00167 }
00168
00170 const std::vector<std::string>& getLinkModelNames() const
00171 {
00172 return link_model_names_vector_;
00173 }
00174
00176 const std::vector<LinkModel*>& getLinkModelsWithCollisionGeometry() const
00177 {
00178 return link_models_with_collision_geometry_vector_;
00179 }
00180
00182 const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
00183 {
00184 return link_model_names_with_collision_geometry_vector_;
00185 }
00186
00190 const JointModel* getRoot() const
00191 {
00192 return root_joint_;
00193 }
00194
00195 const std::string& getRootJointName() const;
00196
00198 const LinkModel* getRootLink() const
00199 {
00200 return root_link_;
00201 }
00202
00204 const std::string& getRootLinkName() const;
00205
00210 const std::string& getModelFrame() const
00211 {
00212 return model_frame_;
00213 }
00214
00216 void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const;
00217
00219 void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const;
00220
00222 void getVariableDefaultValues(std::vector<double> &values) const;
00223
00225 void getVariableDefaultValues(std::map<std::string, double> &values) const;
00226
00228 void printModelInfo(std::ostream &out = std::cout) const;
00229
00231 bool hasJointModelGroup(const std::string& group) const;
00232
00234 const JointModelGroup* getJointModelGroup(const std::string& name) const;
00235
00237 JointModelGroup* getJointModelGroup(const std::string& name);
00238
00240 const std::map<std::string, JointModelGroup*>& getJointModelGroupMap() const
00241 {
00242 return joint_model_group_map_;
00243 }
00244
00246 const std::vector<std::string>& getJointModelGroupNames() const
00247 {
00248 return joint_model_group_names_;
00249 }
00250
00252 const std::map<std::string, srdf::Model::Group>& getJointModelGroupConfigMap() const
00253 {
00254 return joint_model_group_config_map_;
00255 }
00256
00258 bool hasEndEffector(const std::string& eef) const;
00259
00261 const JointModelGroup* getEndEffector(const std::string& name) const;
00262
00264 JointModelGroup* getEndEffector(const std::string& name);
00265
00267 const std::map<std::string, JointModelGroup*>& getEndEffectorsMap() const
00268 {
00269 return end_effectors_;
00270 }
00271
00273 unsigned int getVariableCount() const
00274 {
00275 return variable_count_;
00276 }
00277
00281 const std::vector<std::string>& getVariableNames() const
00282 {
00283 return active_variable_names_;
00284 }
00285
00287 const std::map<std::string, std::pair<double, double> >& getAllVariableBounds() const
00288 {
00289 return variable_bounds_;
00290 }
00291
00296 const std::map<std::string, unsigned int>& getJointVariablesIndexMap() const
00297 {
00298 return joint_variables_index_map_;
00299 }
00300
00302 void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators);
00303
00304 protected:
00305
00306 typedef std::map<LinkModel*, Eigen::Affine3d, std::less<LinkModel*>,
00307 Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > > LinkModelToAffine3dMap;
00308
00309 void computeFixedTransforms(LinkModel *link, const Eigen::Affine3d &transform, LinkModelToAffine3dMap &associated_transforms);
00310
00312 std::string model_name_;
00313
00315 std::string model_frame_;
00316
00318 std::map<std::string, LinkModel*> link_model_map_;
00319
00321 std::vector<LinkModel*> link_model_vector_;
00322
00324 std::vector<const LinkModel*> link_model_vector_const_;
00325
00327 std::vector<std::string> link_model_names_vector_;
00328
00330 std::vector<LinkModel*> link_models_with_collision_geometry_vector_;
00331
00333 std::vector<std::string> link_model_names_with_collision_geometry_vector_;
00334
00336 std::map<std::string, JointModel*> joint_model_map_;
00337
00339 std::vector<JointModel*> joint_model_vector_;
00340
00342 std::vector<const JointModel*> joint_model_vector_const_;
00343
00345 std::vector<std::string> joint_model_names_vector_;
00346
00348 std::vector<const JointModel*> continuous_joint_model_vector_const_;
00349
00351 std::vector<std::string> active_variable_names_;
00352
00354 unsigned int variable_count_;
00355
00357 std::map<std::string,
00358 std::pair<double, double> > variable_bounds_;
00359
00363 std::map<std::string, unsigned int> joint_variables_index_map_;
00364
00366 JointModel *root_joint_;
00367
00369 LinkModel *root_link_;
00370
00372 std::map<std::string, JointModelGroup*> joint_model_group_map_;
00373
00375 std::vector<std::string> joint_model_group_names_;
00376
00378 std::map<std::string, srdf::Model::Group> joint_model_group_config_map_;
00379
00381 std::map<std::string, JointModelGroup*> end_effectors_;
00382
00383 boost::shared_ptr<const srdf::Model> srdf_;
00384
00385 boost::shared_ptr<const urdf::ModelInterface> urdf_;
00386
00388 void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model);
00389
00391 void buildGroups(const srdf::Model &srdf_model);
00392
00394 void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model);
00395
00397 void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model);
00398
00400 void buildMimic(const urdf::ModelInterface &urdf_model);
00401
00403 void buildGroupStates(const srdf::Model &srdf_model);
00404
00406 void buildJointInfo();
00407
00410 JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model);
00411
00413 bool addJointModelGroup(const srdf::Model::Group& group);
00414
00417 JointModel* constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model);
00418
00420 LinkModel* constructLinkModel(const urdf::Link *urdf_link);
00421
00423 shapes::ShapePtr constructShape(const urdf::Geometry *geom);
00424 };
00425
00426 typedef boost::shared_ptr<RobotModel> RobotModelPtr;
00427 typedef boost::shared_ptr<const RobotModel> RobotModelConstPtr;
00428
00429 }
00430
00431 #endif