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_JOINT_MODEL_
00038 #define MOVEIT_ROBOT_MODEL_JOINT_MODEL_
00039
00040 #include <map>
00041 #include <string>
00042 #include <vector>
00043 #include <utility>
00044 #include <moveit_msgs/JointLimits.h>
00045 #include <random_numbers/random_numbers.h>
00046 #include <console_bridge/console.h>
00047 #include <Eigen/Geometry>
00048
00049 namespace robot_model
00050 {
00051
00052 class LinkModel;
00053
00066 class JointModel
00067 {
00068 friend class RobotModel;
00069 public:
00070
00072 enum JointType
00073 {
00074 UNKNOWN, REVOLUTE, PRISMATIC, PLANAR, FLOATING, FIXED
00075 };
00076
00078 typedef std::vector<std::pair<double, double> > Bounds;
00079
00080
00082 JointModel(const std::string& name);
00083
00084 virtual ~JointModel();
00085
00087 const std::string& getName() const
00088 {
00089 return name_;
00090 }
00091
00093 JointType getType() const
00094 {
00095 return type_;
00096 }
00097
00099 std::string getTypeName() const
00100 {
00101 switch(type_)
00102 {
00103 case UNKNOWN: return "Unkown";
00104 case REVOLUTE: return "Revolute";
00105 case PRISMATIC: return "Prismatic";
00106 case PLANAR: return "Planar";
00107 case FLOATING: return "Floating";
00108 case FIXED: return "Fixed";
00109 default: return "[Unkown]";
00110 }
00111 }
00112
00114 int getTreeIndex() const
00115 {
00116 return tree_index_;
00117 }
00118
00122 const LinkModel* getParentLinkModel() const
00123 {
00124 return parent_link_model_;
00125 }
00126
00128 const LinkModel* getChildLinkModel() const
00129 {
00130 return child_link_model_;
00131 }
00132
00139 const std::vector<std::string>& getVariableNames() const
00140 {
00141 return variable_names_;
00142 }
00143
00145 bool hasVariable(const std::string &variable) const
00146 {
00147 return variable_index_.find(variable) != variable_index_.end();
00148 }
00149
00151 unsigned int getVariableCount() const
00152 {
00153 return variable_names_.size();
00154 }
00155
00158 const std::map<std::string, unsigned int>& getVariableIndexMap() const
00159 {
00160 return variable_index_;
00161 }
00162
00165 const std::vector<std::string>& getLocalVariableNames() const
00166 {
00167 return local_variable_names_;
00168 }
00169
00178 void getVariableDefaultValues(std::map<std::string, double> &values) const
00179 {
00180 getVariableDefaultValues(values, variable_bounds_);
00181 }
00182
00186 void getVariableDefaultValues(std::map<std::string, double> &values, const Bounds &other_bounds) const;
00187
00191 void getVariableDefaultValues(std::vector<double> &values) const
00192 {
00193 getVariableDefaultValues(values, variable_bounds_);
00194 }
00195
00199 virtual void getVariableDefaultValues(std::vector<double> &values, const Bounds &other_bounds) const = 0;
00200
00202 void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const
00203 {
00204 getVariableRandomValues(rng, values, variable_bounds_);
00205 }
00206
00208 void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values, const Bounds &other_bounds) const;
00209
00211 void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
00212 {
00213 getVariableRandomValues(rng, values, variable_bounds_);
00214 }
00215
00217 virtual void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &other_bounds) const = 0;
00218
00220 void getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values,
00221 const std::vector<double> &near, const double distance) const
00222 {
00223 getVariableRandomValuesNearBy(rng, values, variable_bounds_, near, distance);
00224 }
00225
00227 virtual void getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &other_bounds,
00228 const std::vector<double> &near, const double distance) const = 0;
00229
00236 bool satisfiesBounds(const std::vector<double> &values, double margin = 0.0) const
00237 {
00238 return satisfiesBounds(values, variable_bounds_, margin);
00239 }
00240
00242 virtual bool satisfiesBounds(const std::vector<double> &values, const Bounds &other_bounds, double margin) const = 0;
00243
00245 void enforceBounds(std::vector<double> &values) const
00246 {
00247 enforceBounds(values, variable_bounds_);
00248 }
00249
00251 virtual void enforceBounds(std::vector<double> &values, const Bounds &other_bounds) const = 0;
00252
00254 bool getVariableBounds(const std::string& variable, std::pair<double, double>& bounds) const;
00255
00257 const Bounds& getVariableBounds() const
00258 {
00259 return variable_bounds_;
00260 }
00261
00263 bool setVariableBounds(const std::string& variable, const std::pair<double, double>& bounds);
00264
00266 const std::vector<moveit_msgs::JointLimits>& getVariableDefaultLimits() const
00267 {
00268 return default_limits_;
00269 }
00270
00272 void setVariableLimits(const std::vector<moveit_msgs::JointLimits>& jlim);
00273
00275 const std::vector<moveit_msgs::JointLimits>& getVariableLimits() const
00276 {
00277 return user_specified_limits_.empty() ? getVariableDefaultLimits() : user_specified_limits_;
00278 }
00279
00280 virtual void computeDefaultVariableLimits();
00281
00285 virtual double distance(const std::vector<double> &values1, const std::vector<double> &values2) const = 0;
00286
00288 double getDistanceFactor() const
00289 {
00290 return distance_factor_;
00291 }
00292
00294 void setDistanceFactor(double factor)
00295 {
00296 distance_factor_ = factor;
00297 }
00298
00300 virtual unsigned int getStateSpaceDimension() const = 0;
00301
00303 const JointModel* getMimic() const
00304 {
00305 return mimic_;
00306 }
00307
00309 double getMimicOffset() const
00310 {
00311 return mimic_offset_;
00312 }
00313
00315 double getMimicFactor() const
00316 {
00317 return mimic_factor_;
00318 }
00319
00321 const std::vector<const JointModel*>& getMimicRequests() const
00322 {
00323 return mimic_requests_;
00324 }
00325
00327 bool isPassive() const
00328 {
00329 return passive_;
00330 }
00331
00333 double getMaximumVelocity() const
00334 {
00335 return max_velocity_;
00336 }
00337
00341 virtual void interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const = 0;
00342
00344 virtual double getMaximumExtent(const Bounds &other_bounds) const = 0;
00345
00346 double getMaximumExtent() const
00347 {
00348 return getMaximumExtent(variable_bounds_);
00349 }
00350
00355 virtual void computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const = 0;
00356
00358 virtual void computeJointStateValues(const Eigen::Affine3d& transform, std::vector<double> &joint_values) const = 0;
00359
00363 virtual void updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const = 0;
00364
00367 protected:
00368
00370 std::string name_;
00371
00373 JointType type_;
00374
00376 std::vector<std::string> local_variable_names_;
00377
00379 std::vector<std::string> variable_names_;
00380
00382 Bounds variable_bounds_;
00383
00385 double max_velocity_;
00386
00388 std::map<std::string, unsigned int> variable_index_;
00389
00391 LinkModel *parent_link_model_;
00392
00394 LinkModel *child_link_model_;
00395
00397 JointModel *mimic_;
00398
00400 double mimic_factor_;
00401
00403 double mimic_offset_;
00404
00406 std::vector<const JointModel*> mimic_requests_;
00407
00409 bool passive_;
00410
00412 double distance_factor_;
00413
00415 std::vector<moveit_msgs::JointLimits> default_limits_;
00416
00418 std::vector<moveit_msgs::JointLimits> user_specified_limits_;
00419
00421 int tree_index_;
00422 };
00423
00424 }
00425
00426 #endif