joint_model.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
00005  *  Copyright (c) 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49