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_JOINT_MODEL_
00039 #define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_
00040
00041 #include <string>
00042 #include <vector>
00043 #include <map>
00044 #include <iostream>
00045 #include <moveit_msgs/JointLimits.h>
00046 #include <random_numbers/random_numbers.h>
00047 #include <Eigen/Geometry>
00048
00049 namespace moveit
00050 {
00051 namespace core
00052 {
00053
00054 struct VariableBounds
00055 {
00056 VariableBounds() :
00057 min_position_(0.0),
00058 max_position_(0.0),
00059 position_bounded_(false),
00060 min_velocity_(0.0),
00061 max_velocity_(0.0),
00062 velocity_bounded_(false),
00063 min_acceleration_(0.0),
00064 max_acceleration_(0.0),
00065 acceleration_bounded_(false)
00066 {
00067 }
00068
00069 double min_position_;
00070 double max_position_;
00071 bool position_bounded_;
00072
00073 double min_velocity_;
00074 double max_velocity_;
00075 bool velocity_bounded_;
00076
00077 double min_acceleration_;
00078 double max_acceleration_;
00079 bool acceleration_bounded_;
00080 };
00081
00082 class LinkModel;
00083 class JointModel;
00084
00086 typedef std::map<std::string, int> VariableIndexMap;
00087
00089 typedef std::map<std::string, VariableBounds> VariableBoundsMap;
00090
00092 typedef std::map<std::string, JointModel*> JointModelMap;
00093
00095 typedef std::map<std::string, const JointModel*> JointModelMapConst;
00096
00097
00110 class JointModel
00111 {
00112 public:
00113
00115 enum JointType
00116 {
00117 UNKNOWN, REVOLUTE, PRISMATIC, PLANAR, FLOATING, FIXED
00118 };
00119
00121 typedef std::vector<VariableBounds> Bounds;
00122
00124 JointModel(const std::string& name);
00125
00126 virtual ~JointModel();
00127
00129 const std::string& getName() const
00130 {
00131 return name_;
00132 }
00133
00135 JointType getType() const
00136 {
00137 return type_;
00138 }
00139
00141 std::string getTypeName() const;
00142
00146 const LinkModel* getParentLinkModel() const
00147 {
00148 return parent_link_model_;
00149 }
00150
00152 const LinkModel* getChildLinkModel() const
00153 {
00154 return child_link_model_;
00155 }
00156
00157 void setParentLinkModel(const LinkModel *link)
00158 {
00159 parent_link_model_ = link;
00160 }
00161
00162 void setChildLinkModel(const LinkModel *link)
00163 {
00164 child_link_model_ = link;
00165 }
00166
00173 const std::vector<std::string>& getVariableNames() const
00174 {
00175 return variable_names_;
00176 }
00177
00180 const std::vector<std::string>& getLocalVariableNames() const
00181 {
00182 return local_variable_names_;
00183 }
00184
00186 bool hasVariable(const std::string &variable) const
00187 {
00188 return variable_index_map_.find(variable) != variable_index_map_.end();
00189 }
00190
00192 std::size_t getVariableCount() const
00193 {
00194 return variable_names_.size();
00195 }
00196
00198 int getFirstVariableIndex() const
00199 {
00200 return first_variable_index_;
00201 }
00202
00204 void setFirstVariableIndex(int index)
00205 {
00206 first_variable_index_ = index;
00207 }
00208
00210 int getJointIndex() const
00211 {
00212 return joint_index_;
00213 }
00214
00216 void setJointIndex(int index)
00217 {
00218 joint_index_ = index;
00219 }
00220
00222 int getLocalVariableIndex(const std::string &variable) const;
00223
00232 void getVariableDefaultPositions(double *values) const
00233 {
00234 getVariableDefaultPositions(values, variable_bounds_);
00235 }
00236
00240 virtual void getVariableDefaultPositions(double *values, const Bounds &other_bounds) const = 0;
00241
00243 void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const
00244 {
00245 getVariableRandomPositions(rng, values, variable_bounds_);
00246 }
00247
00249 virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const = 0;
00250
00252 void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values,
00253 const double *near, const double distance) const
00254 {
00255 getVariableRandomPositionsNearBy(rng, values, variable_bounds_, near, distance);
00256 }
00257
00259 virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds,
00260 const double *near, const double distance) const = 0;
00261
00268 bool satisfiesPositionBounds(const double *values, double margin = 0.0) const
00269 {
00270 return satisfiesPositionBounds(values, variable_bounds_, margin);
00271 }
00272
00274 virtual bool satisfiesPositionBounds(const double *values, const Bounds &other_bounds, double margin) const = 0;
00275
00278 bool enforcePositionBounds(double *values) const
00279 {
00280 return enforcePositionBounds(values, variable_bounds_);
00281 }
00282
00285 virtual bool enforcePositionBounds(double *values, const Bounds &other_bounds) const = 0;
00286
00288 bool satisfiesVelocityBounds(const double *values, double margin = 0.0) const
00289 {
00290 return satisfiesVelocityBounds(values, variable_bounds_, margin);
00291 }
00292
00294 virtual bool satisfiesVelocityBounds(const double *values, const Bounds &other_bounds, double margin) const;
00295
00297 bool enforceVelocityBounds(double *values) const
00298 {
00299 return enforceVelocityBounds(values, variable_bounds_);
00300 }
00301
00303 virtual bool enforceVelocityBounds(double *values, const Bounds &other_bounds) const;
00304
00306 const VariableBounds& getVariableBounds(const std::string& variable) const;
00307
00309 const Bounds& getVariableBounds() const
00310 {
00311 return variable_bounds_;
00312 }
00313
00315 void setVariableBounds(const std::string& variable, const VariableBounds& bounds);
00316
00318 void setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim);
00319
00321 const std::vector<moveit_msgs::JointLimits>& getVariableBoundsMsg() const
00322 {
00323 return variable_bounds_msg_;
00324 }
00325
00329 virtual double distance(const double *value1, const double *value2) const = 0;
00330
00332 double getDistanceFactor() const
00333 {
00334 return distance_factor_;
00335 }
00336
00338 void setDistanceFactor(double factor)
00339 {
00340 distance_factor_ = factor;
00341 }
00342
00344 virtual unsigned int getStateSpaceDimension() const = 0;
00345
00347 const JointModel* getMimic() const
00348 {
00349 return mimic_;
00350 }
00351
00353 double getMimicOffset() const
00354 {
00355 return mimic_offset_;
00356 }
00357
00359 double getMimicFactor() const
00360 {
00361 return mimic_factor_;
00362 }
00363
00365 void setMimic(const JointModel *mimic, double factor, double offset);
00366
00368 const std::vector<const JointModel*>& getMimicRequests() const
00369 {
00370 return mimic_requests_;
00371 }
00372
00374 void addMimicRequest(const JointModel *joint);
00375 void addDescendantJointModel(const JointModel *joint);
00376 void addDescendantLinkModel(const LinkModel *link);
00377
00379 const std::vector<const LinkModel*>& getDescendantLinkModels() const
00380 {
00381 return descendant_link_models_;
00382 }
00383
00385 const std::vector<const JointModel*>& getDescendantJointModels() const
00386 {
00387 return descendant_joint_models_;
00388 }
00389
00391 const std::vector<const JointModel*>& getNonFixedDescendantJointModels() const
00392 {
00393 return non_fixed_descendant_joint_models_;
00394 }
00395
00397 bool isPassive() const
00398 {
00399 return passive_;
00400 }
00401
00402 void setPassive(bool flag)
00403 {
00404 passive_ = flag;
00405 }
00406
00410 virtual void interpolate(const double *from, const double *to, const double t, double *state) const = 0;
00411
00413 virtual double getMaximumExtent(const Bounds &other_bounds) const = 0;
00414
00415 double getMaximumExtent() const
00416 {
00417 return getMaximumExtent(variable_bounds_);
00418 }
00419
00424 virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const = 0;
00425
00427 virtual void computeVariablePositions(const Eigen::Affine3d& transform, double *joint_values) const = 0;
00428
00431 protected:
00432
00433 void computeVariableBoundsMsg();
00434
00436 std::string name_;
00437
00439 JointType type_;
00440
00442 std::vector<std::string> local_variable_names_;
00443
00445 std::vector<std::string> variable_names_;
00446
00448 Bounds variable_bounds_;
00449
00450 std::vector<moveit_msgs::JointLimits> variable_bounds_msg_;
00451
00453 VariableIndexMap variable_index_map_;
00454
00456 const LinkModel *parent_link_model_;
00457
00459 const LinkModel *child_link_model_;
00460
00462 const JointModel *mimic_;
00463
00465 double mimic_factor_;
00466
00468 double mimic_offset_;
00469
00471 std::vector<const JointModel*> mimic_requests_;
00472
00474 std::vector<const LinkModel*> descendant_link_models_;
00475
00477 std::vector<const JointModel*> descendant_joint_models_;
00478
00480 std::vector<const JointModel*> non_fixed_descendant_joint_models_;
00481
00483 bool passive_;
00484
00486 double distance_factor_;
00487
00489 int first_variable_index_;
00490
00492 int joint_index_;
00493
00494 };
00495
00497 std::ostream& operator<<(std::ostream &out, const VariableBounds &b);
00498
00499 }
00500 }
00501
00502 #endif