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
00038 #ifndef MOVEIT_CORE_ROBOT_MODEL_
00039 #define MOVEIT_CORE_ROBOT_MODEL_
00040
00041 #include <moveit/macros/class_forward.h>
00042 #include <moveit/exceptions/exceptions.h>
00043 #include <console_bridge/console.h>
00044 #include <urdf_model/model.h>
00045 #include <srdfdom/model.h>
00046
00047
00048 #include <moveit/robot_model/joint_model_group.h>
00049 #include <moveit/robot_model/fixed_joint_model.h>
00050 #include <moveit/robot_model/floating_joint_model.h>
00051 #include <moveit/robot_model/planar_joint_model.h>
00052 #include <moveit/robot_model/revolute_joint_model.h>
00053 #include <moveit/robot_model/prismatic_joint_model.h>
00054
00055 #include <Eigen/Geometry>
00056 #include <iostream>
00057
00059 namespace moveit
00060 {
00062 namespace core
00063 {
00064
00067 class RobotModel
00068 {
00069 public:
00070
00072 RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00073 const boost::shared_ptr<const srdf::Model> &srdf_model);
00074
00076 ~RobotModel();
00077
00079 const std::string& getName() const
00080 {
00081 return model_name_;
00082 }
00083
00088 const std::string& getModelFrame() const
00089 {
00090 return model_frame_;
00091 }
00092
00094 bool isEmpty() const
00095 {
00096 return root_link_ == NULL;
00097 }
00098
00100 const boost::shared_ptr<const urdf::ModelInterface>& getURDF() const
00101 {
00102 return urdf_;
00103 }
00104
00106 const boost::shared_ptr<const srdf::Model>& getSRDF() const
00107 {
00108 return srdf_;
00109 }
00110
00112 void printModelInfo(std::ostream &out) const;
00113
00122 const JointModel* getRootJoint() const;
00123
00125 const std::string& getRootJointName() const
00126 {
00127 return getRootJoint()->getName();
00128 }
00129
00131 bool hasJointModel(const std::string &name) const;
00132
00134 const JointModel* getJointModel(const std::string &joint) const;
00135
00137 const JointModel* getJointModel(int index) const;
00138
00140 JointModel* getJointModel(const std::string &joint);
00141
00144 const std::vector<const JointModel*>& getJointModels() const
00145 {
00146 return joint_model_vector_const_;
00147 }
00148
00152 const std::vector<JointModel*>& getJointModels()
00153 {
00154 return joint_model_vector_;
00155 }
00156
00158 const std::vector<std::string>& getJointModelNames() const
00159 {
00160 return joint_model_names_vector_;
00161 }
00162
00164 const std::vector<const JointModel*>& getActiveJointModels() const
00165 {
00166 return active_joint_model_vector_const_;
00167 }
00168
00170 const std::vector<JointModel*>& getActiveJointModels()
00171 {
00172 return active_joint_model_vector_;
00173 }
00174
00176 const std::vector<const JointModel*>& getSingleDOFJointModels() const
00177 {
00178 return single_dof_joints_;
00179 }
00180
00182 const std::vector<const JointModel*>& getMultiDOFJointModels() const
00183 {
00184 return multi_dof_joints_;
00185 }
00186
00189 const std::vector<const JointModel*>& getContinuousJointModels() const
00190 {
00191 return continuous_joint_model_vector_;
00192 }
00193
00196 const std::vector<const JointModel*>& getMimicJointModels() const
00197 {
00198 return mimic_joints_;
00199 }
00200
00201 const JointModel* getJointOfVariable(int variable_index) const
00202 {
00203 return joints_of_variable_[variable_index];
00204 }
00205
00206 const JointModel* getJointOfVariable(const std::string &variable) const
00207 {
00208 return joints_of_variable_[getVariableIndex(variable)];
00209 }
00210
00211 std::size_t getJointModelCount() const
00212 {
00213 return joint_model_vector_.size();
00214 }
00215
00223 const LinkModel* getRootLink() const;
00224
00226 const std::string& getRootLinkName() const
00227 {
00228 return getRootLink()->getName();
00229 }
00230
00232 bool hasLinkModel(const std::string &name) const;
00233
00235 const LinkModel* getLinkModel(const std::string &link) const;
00236
00238 const LinkModel* getLinkModel(int index) const;
00239
00241 LinkModel* getLinkModel(const std::string &link);
00242
00244 const std::vector<const LinkModel*>& getLinkModels() const
00245 {
00246 return link_model_vector_const_;
00247 }
00248
00250 const std::vector<LinkModel*>& getLinkModels()
00251 {
00252 return link_model_vector_;
00253 }
00254
00256 const std::vector<std::string>& getLinkModelNames() const
00257 {
00258 return link_model_names_vector_;
00259 }
00260
00262 const std::vector<const LinkModel*>& getLinkModelsWithCollisionGeometry() const
00263 {
00264 return link_models_with_collision_geometry_vector_;
00265 }
00266
00268 const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
00269 {
00270 return link_model_names_with_collision_geometry_vector_;
00271 }
00272
00273 std::size_t getLinkModelCount() const
00274 {
00275 return link_model_vector_.size();
00276 }
00277
00278 std::size_t getLinkGeometryCount() const
00279 {
00280 return link_geometry_count_;
00281 }
00282
00287 void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const;
00288
00290 void getVariableDefaultPositions(double *values) const;
00291
00293 void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
00294 {
00295 values.resize(variable_count_);
00296 getVariableRandomPositions(rng, &values[0]);
00297 }
00298
00300 void getVariableDefaultPositions(std::vector<double> &values) const
00301 {
00302 values.resize(variable_count_);
00303 getVariableDefaultPositions(&values[0]);
00304 }
00305
00307 void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const;
00308
00310 void getVariableDefaultPositions(std::map<std::string, double> &values) const;
00311
00312 bool enforcePositionBounds(double *state) const
00313 {
00314 return enforcePositionBounds(state, active_joint_models_bounds_);
00315 }
00316 bool enforcePositionBounds(double *state, const JointBoundsVector &active_joint_bounds) const;
00317 bool satisfiesPositionBounds(const double *state, double margin = 0.0) const
00318 {
00319 return satisfiesPositionBounds(state, active_joint_models_bounds_, margin);
00320 }
00321 bool satisfiesPositionBounds(const double *state, const JointBoundsVector &active_joint_bounds, double margin = 0.0) const;
00322 double getMaximumExtent() const
00323 {
00324 return getMaximumExtent(active_joint_models_bounds_);
00325 }
00326 double getMaximumExtent(const JointBoundsVector &active_joint_bounds) const;
00327
00328 double distance(const double *state1, const double *state2) const;
00329 void interpolate(const double *from, const double *to, double t, double *state) const;
00330
00336 bool hasJointModelGroup(const std::string& group) const;
00337
00339 const JointModelGroup* getJointModelGroup(const std::string& name) const;
00340
00342 JointModelGroup* getJointModelGroup(const std::string& name);
00343
00345 const std::vector<const JointModelGroup*>& getJointModelGroups() const
00346 {
00347 return joint_model_groups_const_;
00348 }
00349
00351 const std::vector<JointModelGroup*>& getJointModelGroups()
00352 {
00353 return joint_model_groups_;
00354 }
00355
00357 const std::vector<std::string>& getJointModelGroupNames() const
00358 {
00359 return joint_model_group_names_;
00360 }
00361
00363 bool hasEndEffector(const std::string& eef) const;
00364
00366 const JointModelGroup* getEndEffector(const std::string& name) const;
00367
00369 JointModelGroup* getEndEffector(const std::string& name);
00370
00372 const std::vector<const JointModelGroup*>& getEndEffectors() const
00373 {
00374 return end_effectors_;
00375 }
00376
00380 std::size_t getVariableCount() const
00381 {
00382 return variable_count_;
00383 }
00384
00387 const std::vector<std::string>& getVariableNames() const
00388 {
00389 return variable_names_;
00390 }
00391
00393 const VariableBounds& getVariableBounds(const std::string& variable) const
00394 {
00395 return getJointOfVariable(variable)->getVariableBounds(variable);
00396 }
00397
00399 const JointBoundsVector& getActiveJointModelsBounds() const
00400 {
00401 return active_joint_models_bounds_;
00402 }
00403
00404 void getMissingVariableNames(const std::vector<std::string> &variables, std::vector<std::string> &missing_variables) const;
00405
00407 int getVariableIndex(const std::string &variable) const;
00408
00410 const JointModel* getCommonRoot(const JointModel *a, const JointModel *b) const
00411 {
00412 if (!a)
00413 return b;
00414 if (!b)
00415 return a;
00416 return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]];
00417 }
00418
00420 void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators);
00421
00422 protected:
00423
00424 void computeFixedTransforms(const LinkModel *link, const Eigen::Affine3d &transform, LinkTransformMap &associated_transforms);
00425
00427 const JointModel* computeCommonRoot(const JointModel *a, const JointModel *b) const;
00428
00430 void updateMimicJoints(double *values) const;
00431
00432
00433
00435 std::string model_name_;
00436
00438 std::string model_frame_;
00439
00440 boost::shared_ptr<const srdf::Model> srdf_;
00441
00442 boost::shared_ptr<const urdf::ModelInterface> urdf_;
00443
00444
00445
00446
00448 const LinkModel *root_link_;
00449
00451 LinkModelMap link_model_map_;
00452
00454 std::vector<LinkModel*> link_model_vector_;
00455
00457 std::vector<const LinkModel*> link_model_vector_const_;
00458
00460 std::vector<std::string> link_model_names_vector_;
00461
00463 std::vector<const LinkModel*> link_models_with_collision_geometry_vector_;
00464
00466 std::vector<std::string> link_model_names_with_collision_geometry_vector_;
00467
00469 std::size_t link_geometry_count_;
00470
00471
00472
00474 const JointModel *root_joint_;
00475
00477 JointModelMap joint_model_map_;
00478
00480 std::vector<JointModel*> joint_model_vector_;
00481
00483 std::vector<const JointModel*> joint_model_vector_const_;
00484
00486 std::vector<std::string> joint_model_names_vector_;
00487
00489 std::vector<JointModel*> active_joint_model_vector_;
00490
00492 std::vector<const JointModel*> active_joint_model_vector_const_;
00493
00495 std::vector<const JointModel*> continuous_joint_model_vector_;
00496
00498 std::vector<const JointModel*> mimic_joints_;
00499
00500 std::vector<const JointModel*> single_dof_joints_;
00501
00502 std::vector<const JointModel*> multi_dof_joints_;
00503
00511 std::vector<int> common_joint_roots_;
00512
00513
00514
00516 std::vector<std::string> variable_names_;
00517
00519 std::size_t variable_count_;
00520
00524 VariableIndexMap joint_variables_index_map_;
00525
00526 std::vector<int> active_joint_model_start_index_;
00527
00529 JointBoundsVector active_joint_models_bounds_;
00530
00532 std::vector<const JointModel*> joints_of_variable_;
00533
00534
00535
00537 JointModelGroupMap joint_model_group_map_;
00538
00540 JointModelGroupMap end_effectors_map_;
00541
00543 std::vector<JointModelGroup*> joint_model_groups_;
00544
00546 std::vector<const JointModelGroup*> joint_model_groups_const_;
00547
00549 std::vector<std::string> joint_model_group_names_;
00550
00552 std::vector<const JointModelGroup*> end_effectors_;
00553
00555 void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model);
00556
00558 void buildGroups(const srdf::Model &srdf_model);
00559
00561 void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model);
00562
00564 void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model);
00565
00567 void buildMimic(const urdf::ModelInterface &urdf_model);
00568
00570 void buildGroupStates(const srdf::Model &srdf_model);
00571
00573 void buildJointInfo();
00574
00576 void computeDescendants();
00577
00579 void computeCommonRoots();
00580
00583 JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model);
00584
00586 bool addJointModelGroup(const srdf::Model::Group& group);
00587
00590 JointModel* constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model);
00591
00593 LinkModel* constructLinkModel(const urdf::Link *urdf_link);
00594
00596 shapes::ShapePtr constructShape(const urdf::Geometry *geom);
00597 };
00598
00599 MOVEIT_CLASS_FORWARD(RobotModel);
00600
00601 }
00602 }
00603
00604 namespace robot_model=moveit::core;
00605 namespace robot_state=moveit::core;
00606
00607 #endif