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
00037 #ifndef PLANNING_MODELS_KINEMATIC_MODEL_
00038 #define PLANNING_MODELS_KINEMATIC_MODEL_
00039
00040 #include <geometric_shapes/shapes.h>
00041
00042 #include <urdf/model.h>
00043 #include <tf/LinearMath/Transform.h>
00044 #include <boost/thread/shared_mutex.hpp>
00045
00046 #include <iostream>
00047 #include <vector>
00048 #include <string>
00049 #include <map>
00050 #include <boost/bimap.hpp>
00051
00053 namespace planning_models
00054 {
00055
00058 class KinematicModel
00059 {
00060 public:
00062 class JointModel;
00063
00065 class LinkModel;
00066
00068 class AttachedBodyModel;
00069
00070 struct MultiDofConfig
00071 {
00072 MultiDofConfig(std::string n) : name(n) {
00073 }
00074
00075 ~MultiDofConfig() {
00076 }
00077
00079 std::string name;
00080
00082 std::string type;
00083
00085 std::string parent_frame_id;
00086
00088 std::string child_frame_id;
00089
00091 std::map<std::string, std::string> name_equivalents;
00092 };
00093
00094 struct GroupConfig
00095 {
00096 GroupConfig() {
00097 }
00098
00099 GroupConfig(std::string name,
00100 std::string base_link,
00101 std::string tip_link)
00102 : name_(name), base_link_(base_link), tip_link_(tip_link)
00103 {
00104 }
00105
00106 GroupConfig(std::string name,
00107 std::vector<std::string> joints,
00108 std::vector<std::string> subgroups)
00109 : name_(name)
00110 {
00111 joints_ = joints;
00112 subgroups_ = subgroups;
00113 }
00114
00115 std::string name_;
00116 std::string base_link_;
00117 std::string tip_link_;
00118 std::vector<std::string> joints_;
00119 std::vector<std::string> subgroups_;
00120 };
00121
00123 class JointModel
00124 {
00125 friend class KinematicModel;
00126 public:
00127 JointModel(const std::string& name);
00128
00129 JointModel(const JointModel* joint);
00130
00131 void initialize(const std::vector<std::string>& local_names,
00132 const MultiDofConfig* multi_dof_config = NULL);
00133
00134 virtual ~JointModel(void);
00135
00136 typedef boost::bimap< std::string, std::string > js_type;
00137
00138 const std::string& getName() const
00139 {
00140 return name_;
00141 }
00142
00143 const LinkModel* getParentLinkModel() const
00144 {
00145 return parent_link_model_;
00146 }
00147
00148 const LinkModel* getChildLinkModel() const
00149 {
00150 return child_link_model_;
00151 }
00152
00153 const std::string& getParentFrameId() const
00154 {
00155 return parent_frame_id_;
00156 }
00157
00158 const std::string& getChildFrameId() const
00159 {
00160 return child_frame_id_;
00161 }
00162
00163 const js_type& getJointStateEquivalents() const
00164 {
00165 return joint_state_equivalents_;
00166 }
00167
00168 const std::map<unsigned int, std::string>& getComputatationOrderMapIndex() const
00169 {
00170 return computation_order_map_index_;
00171 }
00173 std::string getEquiv(const std::string& name) const;
00174
00176 bool getVariableBounds(const std::string& variable, std::pair<double, double>& bounds) const;
00177
00179 bool setVariableBounds(const std::string& variable, double low, double high);
00180
00184 virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;
00185
00186 virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const;
00187
00188 const std::map<std::string, std::pair<double, double> >& getAllVariableBounds() const {
00189 return joint_state_bounds_;
00190 }
00191
00192 bool hasVariable(const std::string var) const
00193 {
00194 return(joint_state_equivalents_.right.find(var) != joint_state_equivalents_.right.end());
00195 }
00196
00197 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const = 0;
00198
00199 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const = 0;
00200
00201 private:
00202
00204 std::string name_;
00205
00207 LinkModel *parent_link_model_;
00208
00210 LinkModel *child_link_model_;
00211
00212
00213 js_type joint_state_equivalents_;
00214
00215
00216 std::map<std::string, std::pair<double, double> > joint_state_bounds_;
00217
00218
00219 std::map<unsigned int, std::string> computation_order_map_index_;
00220
00222 std::string parent_frame_id_;
00223
00225 std::string child_frame_id_;
00226 };
00227
00229 class FixedJointModel : public JointModel
00230 {
00231 public:
00232
00233 FixedJointModel(const std::string name, const MultiDofConfig* multi_dof_config) :
00234 JointModel(name)
00235 {
00236 std::vector<std::string> local_names;
00237 initialize(local_names, multi_dof_config);
00238 }
00239
00240 FixedJointModel(const FixedJointModel* joint): JointModel(joint)
00241 {
00242 }
00243
00244 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const {
00245 tf::Transform ident;
00246 ident.setIdentity();
00247 return ident;
00248 }
00249
00250 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const {
00251 std::vector<double> ret;
00252 return ret;
00253 }
00254
00255 };
00256
00258 class PlanarJointModel : public JointModel
00259 {
00260 public:
00261
00262 PlanarJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00263
00264 PlanarJointModel(const PlanarJointModel* joint): JointModel(joint)
00265 {
00266 }
00267
00268 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00269
00270 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00271
00272 };
00273
00275 class FloatingJointModel : public JointModel
00276 {
00277 public:
00278
00279 FloatingJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00280
00281 FloatingJointModel(const FloatingJointModel* joint) : JointModel(joint)
00282 {
00283 }
00284
00285 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00286
00287 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00288
00289 virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;
00290
00291 };
00292
00294 class PrismaticJointModel : public JointModel
00295 {
00296 public:
00297
00298 PrismaticJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00299
00300 PrismaticJointModel(const PrismaticJointModel* joint) : JointModel(joint){
00301 axis_ = joint->axis_;
00302 }
00303
00304 tf::Vector3 axis_;
00305
00306 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00307
00308 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00309
00310 };
00311
00313 class RevoluteJointModel : public JointModel
00314 {
00315 public:
00316
00317 RevoluteJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00318
00319 RevoluteJointModel(const RevoluteJointModel* joint) : JointModel(joint){
00320 axis_ = joint->axis_;
00321 continuous_ = joint->continuous_;
00322 }
00323
00324 tf::Vector3 axis_;
00325 bool continuous_;
00326
00327 virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00328
00329 virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00330
00331
00332 virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const;
00333 };
00334
00336 class LinkModel
00337 {
00338 friend class KinematicModel;
00339 public:
00340
00341 LinkModel(const KinematicModel* kinematic_model);
00342
00343 LinkModel(const LinkModel* link_model);
00344
00345 ~LinkModel(void);
00346
00347 const std::string& getName() const {
00348 return name_;
00349 }
00350
00351 const JointModel* getParentJointModel() const {
00352 return parent_joint_model_;
00353 }
00354
00355 const std::vector<JointModel*>& getChildJointModels() const {
00356 return child_joint_models_;
00357 }
00358
00359 const tf::Transform& getJointOriginTransform() const {
00360 return joint_origin_transform_;
00361 }
00362
00363 const tf::Transform& getCollisionOriginTransform() const {
00364 return collision_origin_transform_;
00365 }
00366
00367 const shapes::Shape* getLinkShape() const {
00368 return shape_;
00369 }
00370
00371 const std::vector<AttachedBodyModel*>& getAttachedBodyModels() const {
00372 return attached_body_models_;
00373 }
00374
00375 private:
00376
00378 void clearAttachedBodyModels();
00379
00383 void replaceAttachedBodyModels(std::vector<AttachedBodyModel*>& attached_body_vector);
00384
00385 void clearLinkAttachedBodyModel(const std::string& att_name);
00386
00387 void addAttachedBodyModel(AttachedBodyModel* attached_body_model);
00388
00390 std::string name_;
00391
00393 const KinematicModel* kinematic_model_;
00394
00396 JointModel *parent_joint_model_;
00397
00399 std::vector<JointModel*> child_joint_models_;
00400
00402 tf::Transform joint_origin_transform_;
00403
00405 tf::Transform collision_origin_transform_;
00406
00408 shapes::Shape *shape_;
00409
00411 std::vector<AttachedBodyModel*> attached_body_models_;
00412 };
00413
00417 class AttachedBodyModel
00418 {
00419 public:
00420
00421 AttachedBodyModel(const LinkModel *link,
00422 const std::string& id,
00423 const std::vector<tf::Transform>& attach_trans,
00424 const std::vector<std::string>& touch_links,
00425 std::vector<shapes::Shape*>& shapes);
00426
00427 ~AttachedBodyModel(void);
00428
00429 const std::string& getName() const
00430 {
00431 return id_;
00432 }
00433
00434 const LinkModel* getAttachedLinkModel() const
00435 {
00436 return attached_link_model_;
00437 }
00438
00439 const std::vector<shapes::Shape*>& getShapes() const
00440 {
00441 return shapes_;
00442 }
00443
00444 const std::vector<tf::Transform>& getAttachedBodyFixedTransforms() const
00445 {
00446 return attach_trans_;
00447 }
00448
00449 const std::vector<std::string>& getTouchLinks() const
00450 {
00451 return touch_links_;
00452 }
00453
00454 private:
00455
00457 const LinkModel *attached_link_model_;
00458
00460 std::vector<shapes::Shape*> shapes_;
00461
00463 std::vector<tf::Transform> attach_trans_;
00464
00466 std::vector<std::string> touch_links_;
00467
00469 std::string id_;
00470 };
00471
00472 class JointModelGroup
00473 {
00474 friend class KinematicModel;
00475 public:
00476
00477 JointModelGroup(const std::string& name,
00478 const std::vector<const JointModel*>& joint_vector,
00479 const std::vector<const JointModel*>& fixed_joint_vector,
00480 const KinematicModel* parent_model);
00481
00482 ~JointModelGroup(void);
00483
00484 const std::string& getName() const
00485 {
00486 return name_;
00487 }
00488
00490 bool hasJointModel(const std::string &joint) const;
00491
00493 const JointModel* getJointModel(const std::string &joint);
00494
00495 const std::vector<const JointModel*>& getJointModels() const
00496 {
00497 return joint_model_vector_;
00498 }
00499
00500 const std::vector<const JointModel*>& getFixedJointModels() const
00501 {
00502 return fixed_joint_model_vector_;
00503 }
00504
00505 const std::vector<std::string>& getJointModelNames() const
00506 {
00507 return joint_model_name_vector_;
00508 }
00509
00510 const std::vector<const JointModel*>& getJointRoots() const
00511 {
00512 return joint_roots_;
00513 }
00514
00515 const std::vector<const LinkModel*>& getGroupLinkModels() const
00516 {
00517 return group_link_model_vector_;
00518 }
00519
00520 std::vector<std::string> getGroupLinkNames() const
00521 {
00522 std::vector<std::string> ret_vec;
00523 for(unsigned int i = 0; i < group_link_model_vector_.size(); i++) {
00524 ret_vec.push_back(group_link_model_vector_[i]->getName());
00525 }
00526 return ret_vec;
00527 }
00528
00529 const std::vector<const LinkModel*>& getUpdatedLinkModels() const
00530 {
00531 return updated_link_model_vector_;
00532 }
00533
00534 std::vector<std::string> getUpdatedLinkModelNames() const
00535 {
00536 std::vector<std::string> ret_vec;
00537 for(unsigned int i = 0; i < updated_link_model_vector_.size(); i++) {
00538 ret_vec.push_back(updated_link_model_vector_[i]->getName());
00539 }
00540 return ret_vec;
00541 }
00542
00543 private:
00544
00545 bool is_valid_;
00546
00548 std::string name_;
00549
00551 std::vector<std::string> joint_model_name_vector_;
00552
00554 std::vector<const JointModel*> joint_model_vector_;
00555
00557 std::vector<const JointModel*> fixed_joint_model_vector_;
00558
00560 std::map<std::string, const JointModel*> joint_model_map_;
00561
00563 std::vector<const JointModel*> joint_roots_;
00564
00568 std::vector<const LinkModel*> group_link_model_vector_;
00569
00571 std::vector<const LinkModel*> updated_link_model_vector_;
00572
00573 };
00574
00576 KinematicModel(const KinematicModel &source);
00577
00579 KinematicModel(const urdf::Model &model,
00580 const std::vector<GroupConfig>& group_configs,
00581 const std::vector<MultiDofConfig>& multi_dof_configs);
00582
00584 ~KinematicModel(void);
00585
00586 void copyFrom(const KinematicModel &source);
00587
00589 const std::string& getName(void) const;
00590
00592 const LinkModel* getLinkModel(const std::string &link) const;
00593
00595 bool hasLinkModel(const std::string &name) const;
00596
00598 void getLinkModelNames(std::vector<std::string> &links) const;
00599
00601 void getChildLinkModels(const LinkModel* parent, std::vector<const LinkModel*> &links) const;
00602
00604 void getChildLinkModels(const JointModel* parent, std::vector<const LinkModel*> &links) const;
00605
00607 void getChildJointModels(const LinkModel* parent, std::vector<const JointModel*> &links) const;
00608
00610 void getChildJointModels(const JointModel* parent, std::vector<const JointModel*> &links) const;
00611
00613 std::vector<std::string> getChildLinkModelNames(const LinkModel* parent) const;
00614
00616 std::vector<std::string> getChildJointModelNames(const LinkModel* parent) const;
00617
00619 std::vector<std::string> getChildJointModelNames(const JointModel* parent) const;
00620
00622 std::vector<std::string> getChildLinkModelNames(const JointModel* parent) const;
00623
00625 const JointModel* getJointModel(const std::string &joint) const;
00626
00628 bool hasJointModel(const std::string &name) const;
00629
00632 const std::vector<JointModel*>& getJointModels() const
00633 {
00634 return joint_model_vector_;
00635 }
00638 const std::vector<LinkModel*>& getLinkModels() const
00639 {
00640 return link_model_vector_;
00641 }
00642
00643 const std::vector<LinkModel*>& getLinkModelsWithCollisionGeometry() const
00644 {
00645 return link_models_with_collision_geometry_vector_;
00646 }
00647
00650 void getJointModelNames(std::vector<std::string> &joints) const;
00651
00653 const JointModel* getRoot(void) const;
00654
00656 void exclusiveLock(void) const;
00657
00659 void exclusiveUnlock(void) const;
00660
00662 void sharedLock(void) const;
00663
00665 void sharedUnlock(void) const;
00666
00668 void printModelInfo(std::ostream &out = std::cout) const;
00669
00670 bool hasModelGroup(const std::string& group) const;
00671
00672 bool addModelGroup(const GroupConfig& group);
00673
00674 void removeModelGroup(const std::string& group);
00675
00676 const JointModelGroup* getModelGroup(const std::string& name) const
00677 {
00678 if(!hasModelGroup(name)) return NULL;
00679 return joint_model_group_map_.find(name)->second;
00680 }
00681
00682 const std::map<std::string, JointModelGroup*>& getJointModelGroupMap() const
00683 {
00684 return joint_model_group_map_;
00685 }
00686
00687 const std::map<std::string, GroupConfig>& getJointModelGroupConfigMap() const {
00688 return joint_model_group_config_map_;
00689 }
00690
00691 void getModelGroupNames(std::vector<std::string>& getModelGroupNames) const;
00692
00693 void clearAllAttachedBodyModels();
00694
00695 void clearLinkAttachedBodyModels(const std::string& link_name);
00696
00697 void clearLinkAttachedBodyModel(const std::string& link_name, const std::string& att_name);
00698
00699 void replaceAttachedBodyModels(const std::string& link_name, std::vector<AttachedBodyModel*>& attached_body_vector);
00700
00701 void addAttachedBodyModel(const std::string& link_name, AttachedBodyModel* att_body_model);
00702
00703 std::vector<const AttachedBodyModel*> getAttachedBodyModels() const;
00704
00705 std::string getRobotName() const {
00706 return model_name_;
00707 }
00708
00709 private:
00710
00712 mutable boost::shared_mutex lock_;
00713
00715 std::string model_name_;
00716
00718 std::map<std::string, LinkModel*> link_model_map_;
00719
00721 std::map<std::string, JointModel*> joint_model_map_;
00722
00724 std::vector<JointModel*> joint_model_vector_;
00725
00727 std::vector<LinkModel*> link_model_vector_;
00728
00730 std::vector<LinkModel*> link_models_with_collision_geometry_vector_;
00731
00733 JointModel *root_;
00734
00735 std::map<std::string, JointModelGroup*> joint_model_group_map_;
00736 std::map<std::string, GroupConfig> joint_model_group_config_map_;
00737
00738 void buildGroups(const std::vector<GroupConfig>&);
00739 JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link,
00740 const std::vector<MultiDofConfig>& multi_dof_configs);
00741 JointModel* constructJointModel(const urdf::Joint *urdfJointModel, const urdf::Link *child_link,
00742 const std::vector<MultiDofConfig>& multi_dof_configs);
00743 LinkModel* constructLinkModel(const urdf::Link *urdfLink);
00744 shapes::Shape* constructShape(const urdf::Geometry *geom);
00745
00746 JointModel* copyJointModel(const JointModel *joint);
00747 JointModel* copyRecursive(LinkModel *parent, const LinkModel *link);
00748
00749 };
00750
00751 }
00752
00753 #endif