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 
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52