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 struct VariableBounds
00054 {
00055 VariableBounds()
00056 : min_position_(0.0)
00057 , max_position_(0.0)
00058 , position_bounded_(false)
00059 , min_velocity_(0.0)
00060 , max_velocity_(0.0)
00061 , velocity_bounded_(false)
00062 , min_acceleration_(0.0)
00063 , max_acceleration_(0.0)
00064 , acceleration_bounded_(false)
00065 {
00066 }
00067
00068 double min_position_;
00069 double max_position_;
00070 bool position_bounded_;
00071
00072 double min_velocity_;
00073 double max_velocity_;
00074 bool velocity_bounded_;
00075
00076 double min_acceleration_;
00077 double max_acceleration_;
00078 bool acceleration_bounded_;
00079 };
00080
00081 class LinkModel;
00082 class JointModel;
00083
00085 typedef std::map<std::string, int> VariableIndexMap;
00086
00088 typedef std::map<std::string, VariableBounds> VariableBoundsMap;
00089
00091 typedef std::map<std::string, JointModel*> JointModelMap;
00092
00094 typedef std::map<std::string, const JointModel*> JointModelMapConst;
00095
00108 class JointModel
00109 {
00110 public:
00112 enum JointType
00113 {
00114 UNKNOWN,
00115 REVOLUTE,
00116 PRISMATIC,
00117 PLANAR,
00118 FLOATING,
00119 FIXED
00120 };
00121
00123 typedef std::vector<VariableBounds> Bounds;
00124
00126 JointModel(const std::string& name);
00127
00128 virtual ~JointModel();
00129
00131 const std::string& getName() const
00132 {
00133 return name_;
00134 }
00135
00137 JointType getType() const
00138 {
00139 return type_;
00140 }
00141
00143 std::string getTypeName() const;
00144
00148 const LinkModel* getParentLinkModel() const
00149 {
00150 return parent_link_model_;
00151 }
00152
00154 const LinkModel* getChildLinkModel() const
00155 {
00156 return child_link_model_;
00157 }
00158
00159 void setParentLinkModel(const LinkModel* link)
00160 {
00161 parent_link_model_ = link;
00162 }
00163
00164 void setChildLinkModel(const LinkModel* link)
00165 {
00166 child_link_model_ = link;
00167 }
00168
00176 const std::vector<std::string>& getVariableNames() const
00177 {
00178 return variable_names_;
00179 }
00180
00184 const std::vector<std::string>& getLocalVariableNames() const
00185 {
00186 return local_variable_names_;
00187 }
00188
00190 bool hasVariable(const std::string& variable) const
00191 {
00192 return variable_index_map_.find(variable) != variable_index_map_.end();
00193 }
00194
00196 std::size_t getVariableCount() const
00197 {
00198 return variable_names_.size();
00199 }
00200
00202 int getFirstVariableIndex() const
00203 {
00204 return first_variable_index_;
00205 }
00206
00208 void setFirstVariableIndex(int index)
00209 {
00210 first_variable_index_ = index;
00211 }
00212
00214 int getJointIndex() const
00215 {
00216 return joint_index_;
00217 }
00218
00220 void setJointIndex(int index)
00221 {
00222 joint_index_ = index;
00223 }
00224
00226 int getLocalVariableIndex(const std::string& variable) const;
00227
00236 void getVariableDefaultPositions(double* values) const
00237 {
00238 getVariableDefaultPositions(values, variable_bounds_);
00239 }
00240
00244 virtual void getVariableDefaultPositions(double* values, const Bounds& other_bounds) const = 0;
00245
00248 void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const
00249 {
00250 getVariableRandomPositions(rng, values, variable_bounds_);
00251 }
00252
00255 virtual void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
00256 const Bounds& other_bounds) const = 0;
00257
00260 void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near,
00261 const double distance) const
00262 {
00263 getVariableRandomPositionsNearBy(rng, values, variable_bounds_, near, distance);
00264 }
00265
00268 virtual void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
00269 const Bounds& other_bounds, const double* near,
00270 const double distance) const = 0;
00271
00278 bool satisfiesPositionBounds(const double* values, double margin = 0.0) const
00279 {
00280 return satisfiesPositionBounds(values, variable_bounds_, margin);
00281 }
00282
00285 virtual bool satisfiesPositionBounds(const double* values, const Bounds& other_bounds, double margin) const = 0;
00286
00290 bool enforcePositionBounds(double* values) const
00291 {
00292 return enforcePositionBounds(values, variable_bounds_);
00293 }
00294
00298 virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0;
00299
00301 bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
00302 {
00303 return satisfiesVelocityBounds(values, variable_bounds_, margin);
00304 }
00305
00307 virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const;
00308
00310 bool enforceVelocityBounds(double* values) const
00311 {
00312 return enforceVelocityBounds(values, variable_bounds_);
00313 }
00314
00316 virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const;
00317
00319 const VariableBounds& getVariableBounds(const std::string& variable) const;
00320
00322 const Bounds& getVariableBounds() const
00323 {
00324 return variable_bounds_;
00325 }
00326
00328 void setVariableBounds(const std::string& variable, const VariableBounds& bounds);
00329
00331 void setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim);
00332
00334 const std::vector<moveit_msgs::JointLimits>& getVariableBoundsMsg() const
00335 {
00336 return variable_bounds_msg_;
00337 }
00338
00342 virtual double distance(const double* value1, const double* value2) const = 0;
00343
00346 double getDistanceFactor() const
00347 {
00348 return distance_factor_;
00349 }
00350
00353 void setDistanceFactor(double factor)
00354 {
00355 distance_factor_ = factor;
00356 }
00357
00359 virtual unsigned int getStateSpaceDimension() const = 0;
00360
00362 const JointModel* getMimic() const
00363 {
00364 return mimic_;
00365 }
00366
00368 double getMimicOffset() const
00369 {
00370 return mimic_offset_;
00371 }
00372
00374 double getMimicFactor() const
00375 {
00376 return mimic_factor_;
00377 }
00378
00380 void setMimic(const JointModel* mimic, double factor, double offset);
00381
00383 const std::vector<const JointModel*>& getMimicRequests() const
00384 {
00385 return mimic_requests_;
00386 }
00387
00389 void addMimicRequest(const JointModel* joint);
00390 void addDescendantJointModel(const JointModel* joint);
00391 void addDescendantLinkModel(const LinkModel* link);
00392
00394 const std::vector<const LinkModel*>& getDescendantLinkModels() const
00395 {
00396 return descendant_link_models_;
00397 }
00398
00400 const std::vector<const JointModel*>& getDescendantJointModels() const
00401 {
00402 return descendant_joint_models_;
00403 }
00404
00406 const std::vector<const JointModel*>& getNonFixedDescendantJointModels() const
00407 {
00408 return non_fixed_descendant_joint_models_;
00409 }
00410
00412 bool isPassive() const
00413 {
00414 return passive_;
00415 }
00416
00417 void setPassive(bool flag)
00418 {
00419 passive_ = flag;
00420 }
00421
00426 virtual void interpolate(const double* from, const double* to, const double t, double* state) const = 0;
00427
00429 virtual double getMaximumExtent(const Bounds& other_bounds) const = 0;
00430
00431 double getMaximumExtent() const
00432 {
00433 return getMaximumExtent(variable_bounds_);
00434 }
00435
00440 virtual void computeTransform(const double* joint_values, Eigen::Affine3d& transf) const = 0;
00441
00443 virtual void computeVariablePositions(const Eigen::Affine3d& transform, double* joint_values) const = 0;
00444
00447 protected:
00448 void computeVariableBoundsMsg();
00449
00451 std::string name_;
00452
00454 JointType type_;
00455
00457 std::vector<std::string> local_variable_names_;
00458
00460 std::vector<std::string> variable_names_;
00461
00463 Bounds variable_bounds_;
00464
00465 std::vector<moveit_msgs::JointLimits> variable_bounds_msg_;
00466
00469 VariableIndexMap variable_index_map_;
00470
00472 const LinkModel* parent_link_model_;
00473
00475 const LinkModel* child_link_model_;
00476
00478 const JointModel* mimic_;
00479
00481 double mimic_factor_;
00482
00484 double mimic_offset_;
00485
00487 std::vector<const JointModel*> mimic_requests_;
00488
00490 std::vector<const LinkModel*> descendant_link_models_;
00491
00493 std::vector<const JointModel*> descendant_joint_models_;
00494
00497 std::vector<const JointModel*> non_fixed_descendant_joint_models_;
00498
00500 bool passive_;
00501
00503 double distance_factor_;
00504
00506 int first_variable_index_;
00507
00509 int joint_index_;
00510 };
00511
00513 std::ostream& operator<<(std::ostream& out, const VariableBounds& b);
00514 }
00515 }
00516
00517 #endif