joint_model_group.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) 2008-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_GROUP_
00039 #define MOVEIT_CORE_ROBOT_MODEL_JOINT_MODEL_GROUP_
00040 
00041 #include <moveit/robot_model/joint_model.h>
00042 #include <moveit/robot_model/link_model.h>
00043 #include <moveit/kinematics_base/kinematics_base.h>
00044 #include <srdfdom/model.h>
00045 #include <boost/function.hpp>
00046 #include <set>
00047 
00048 namespace moveit
00049 {
00050 namespace core
00051 {
00052 class RobotModel;
00053 class JointModelGroup;
00054 
00056 typedef boost::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> SolverAllocatorFn;
00057 
00059 typedef std::map<const JointModelGroup*, SolverAllocatorFn> SolverAllocatorMapFn;
00060 
00062 typedef std::map<std::string, JointModelGroup*> JointModelGroupMap;
00063 
00065 typedef std::map<std::string, const JointModelGroup*> JointModelGroupMapConst;
00066 
00067 typedef std::vector<const JointModel::Bounds*> JointBoundsVector;
00068 
00069 class JointModelGroup
00070 {
00071 public:
00072   struct KinematicsSolver
00073   {
00074     KinematicsSolver() : default_ik_timeout_(0.5), default_ik_attempts_(2)
00075     {
00076     }
00077 
00079     operator bool() const
00080     {
00081       return allocator_ && !bijection_.empty() && solver_instance_;
00082     }
00083 
00084     void reset()
00085     {
00086       solver_instance_.reset();
00087       solver_instance_const_.reset();
00088       bijection_.clear();
00089     }
00090 
00092     SolverAllocatorFn allocator_;
00093 
00099     std::vector<unsigned int> bijection_;
00100 
00101     kinematics::KinematicsBaseConstPtr solver_instance_const_;
00102 
00103     kinematics::KinematicsBasePtr solver_instance_;
00104 
00105     double default_ik_timeout_;
00106 
00107     unsigned int default_ik_attempts_;
00108   };
00109 
00111   typedef std::map<const JointModelGroup*, KinematicsSolver> KinematicsSolverMap;
00112 
00113   JointModelGroup(const std::string& name, const srdf::Model::Group& config,
00114                   const std::vector<const JointModel*>& joint_vector, const RobotModel* parent_model);
00115 
00116   ~JointModelGroup();
00117 
00119   const RobotModel& getParentModel() const
00120   {
00121     return *parent_model_;
00122   }
00123 
00125   const std::string& getName() const
00126   {
00127     return name_;
00128   }
00129 
00131   const srdf::Model::Group& getConfig() const
00132   {
00133     return config_;
00134   }
00135 
00137   bool hasJointModel(const std::string& joint) const;
00138 
00140   bool hasLinkModel(const std::string& link) const;
00141 
00143   const JointModel* getJointModel(const std::string& joint) const;
00144 
00146   const LinkModel* getLinkModel(const std::string& link) const;
00147 
00149   const std::vector<const JointModel*>& getJointModels() const
00150   {
00151     return joint_model_vector_;
00152   }
00153 
00156   const std::vector<std::string>& getJointModelNames() const
00157   {
00158     return joint_model_name_vector_;
00159   }
00160 
00162   const std::vector<const JointModel*>& getActiveJointModels() const
00163   {
00164     return active_joint_model_vector_;
00165   }
00166 
00169   const std::vector<std::string>& getActiveJointModelNames() const
00170   {
00171     return active_joint_model_name_vector_;
00172   }
00173 
00175   const std::vector<const JointModel*>& getFixedJointModels() const
00176   {
00177     return fixed_joints_;
00178   }
00179 
00181   const std::vector<const JointModel*>& getMimicJointModels() const
00182   {
00183     return mimic_joints_;
00184   }
00185 
00187   const std::vector<const JointModel*>& getContinuousJointModels() const
00188   {
00189     return continuous_joint_model_vector_;
00190   }
00191 
00194   const std::vector<std::string>& getVariableNames() const
00195   {
00196     return variable_names_;
00197   }
00198 
00206   const std::vector<const JointModel*>& getJointRoots() const
00207   {
00208     return joint_roots_;
00209   }
00210 
00212   const JointModel* getCommonRoot() const
00213   {
00214     return common_root_;
00215   }
00216 
00218   const std::vector<const LinkModel*>& getLinkModels() const
00219   {
00220     return link_model_vector_;
00221   }
00222 
00224   const std::vector<std::string>& getLinkModelNames() const
00225   {
00226     return link_model_name_vector_;
00227   }
00228 
00230   const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
00231   {
00232     return link_model_with_geometry_name_vector_;
00233   }
00234 
00239   const std::vector<const LinkModel*>& getUpdatedLinkModels() const
00240   {
00241     return updated_link_model_vector_;
00242   }
00243 
00245   const std::set<const LinkModel*>& getUpdatedLinkModelsSet() const
00246   {
00247     return updated_link_model_set_;
00248   }
00249 
00251   const std::vector<std::string>& getUpdatedLinkModelNames() const
00252   {
00253     return updated_link_model_name_vector_;
00254   }
00255 
00259   const std::vector<const LinkModel*>& getUpdatedLinkModelsWithGeometry() const
00260   {
00261     return updated_link_model_with_geometry_vector_;
00262   }
00263 
00265   const std::set<const LinkModel*>& getUpdatedLinkModelsWithGeometrySet() const
00266   {
00267     return updated_link_model_with_geometry_set_;
00268   }
00269 
00271   const std::vector<std::string>& getUpdatedLinkModelsWithGeometryNames() const
00272   {
00273     return updated_link_model_with_geometry_name_vector_;
00274   }
00275 
00277   const std::set<std::string>& getUpdatedLinkModelsWithGeometryNamesSet() const
00278   {
00279     return updated_link_model_with_geometry_name_set_;
00280   }
00281 
00285   bool isLinkUpdated(const std::string& name) const
00286   {
00287     return updated_link_model_name_set_.find(name) != updated_link_model_name_set_.end();
00288   }
00289 
00291   const std::vector<int>& getVariableIndexList() const
00292   {
00293     return variable_index_list_;
00294   }
00295 
00297   int getVariableGroupIndex(const std::string& variable) const;
00298 
00300   const std::vector<std::string>& getDefaultStateNames() const
00301   {
00302     return default_states_names_;
00303   }
00304 
00305   void addDefaultState(const std::string& name, const std::map<std::string, double>& default_state);
00306 
00308   bool getVariableDefaultPositions(const std::string& name, std::map<std::string, double>& values) const;
00309 
00311   void getVariableDefaultPositions(std::map<std::string, double>& values) const;
00312 
00314   void getVariableDefaultPositions(std::vector<double>& values) const
00315   {
00316     values.resize(variable_count_);
00317     getVariableDefaultPositions(&values[0]);
00318   }
00319 
00321   void getVariableDefaultPositions(double* values) const;
00322 
00324   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const
00325   {
00326     getVariableRandomPositions(rng, values, active_joint_models_bounds_);
00327   }
00328 
00330   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
00331   {
00332     values.resize(variable_count_);
00333     getVariableRandomPositions(rng, &values[0], active_joint_models_bounds_);
00334   }
00335 
00337   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values, const double* near,
00338                                         const double distance) const
00339   {
00340     getVariableRandomPositionsNearBy(rng, values, active_joint_models_bounds_, near, distance);
00341   }
00343   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
00344                                         const std::vector<double>& near, double distance) const
00345   {
00346     values.resize(variable_count_);
00347     getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance);
00348   }
00349 
00351   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
00352                                         const std::vector<double>& near,
00353                                         const std::map<JointModel::JointType, double>& distance_map) const
00354   {
00355     values.resize(variable_count_);
00356     getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distance_map);
00357   }
00358 
00360   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values,
00361                                         const std::vector<double>& near, const std::vector<double>& distances) const
00362   {
00363     values.resize(variable_count_);
00364     getVariableRandomPositionsNearBy(rng, &values[0], active_joint_models_bounds_, &near[0], distances);
00365   }
00366 
00367   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
00368                                   const JointBoundsVector& active_joint_bounds) const;
00369 
00371   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
00372                                         const JointBoundsVector& active_joint_bounds, const double* near,
00373                                         const double distance) const;
00374 
00376   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
00377                                         const JointBoundsVector& active_joint_bounds, const double* near,
00378                                         const std::map<JointModel::JointType, double>& distance_map) const;
00379 
00381   void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
00382                                         const JointBoundsVector& active_joint_bounds, const double* near,
00383                                         const std::vector<double>& distances) const;
00384 
00385   bool enforcePositionBounds(double* state) const
00386   {
00387     return enforcePositionBounds(state, active_joint_models_bounds_);
00388   }
00389 
00390   bool enforcePositionBounds(double* state, const JointBoundsVector& active_joint_bounds) const;
00391   bool satisfiesPositionBounds(const double* state, double margin = 0.0) const
00392   {
00393     return satisfiesPositionBounds(state, active_joint_models_bounds_, margin);
00394   }
00395   bool satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
00396                                double margin = 0.0) const;
00397 
00398   double getMaximumExtent() const
00399   {
00400     return getMaximumExtent(active_joint_models_bounds_);
00401   }
00402   double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
00403 
00404   double distance(const double* state1, const double* state2) const;
00405   void interpolate(const double* from, const double* to, double t, double* state) const;
00406 
00409   unsigned int getVariableCount() const
00410   {
00411     return variable_count_;
00412   }
00413 
00415   void setSubgroupNames(const std::vector<std::string>& subgroups);
00416 
00418   const std::vector<std::string>& getSubgroupNames() const
00419   {
00420     return subgroup_names_;
00421   }
00422 
00424   void getSubgroups(std::vector<const JointModelGroup*>& sub_groups) const;
00425 
00427   bool isSubgroup(const std::string& group) const
00428   {
00429     return subgroup_names_set_.find(group) != subgroup_names_set_.end();
00430   }
00431 
00433   bool isChain() const
00434   {
00435     return is_chain_;
00436   }
00437 
00439   bool isSingleDOFJoints() const
00440   {
00441     return is_single_dof_;
00442   }
00443 
00445   bool isEndEffector() const
00446   {
00447     return !end_effector_name_.empty();
00448   }
00449 
00450   bool isContiguousWithinState() const
00451   {
00452     return is_contiguous_index_list_;
00453   }
00454 
00456   const std::string& getEndEffectorName() const
00457   {
00458     return end_effector_name_;
00459   }
00460 
00462   void setEndEffectorName(const std::string& name);
00463 
00467   void setEndEffectorParent(const std::string& group, const std::string& link);
00468 
00470   void attachEndEffector(const std::string& eef_name);
00471 
00474   const std::pair<std::string, std::string>& getEndEffectorParentGroup() const
00475   {
00476     return end_effector_parent_;
00477   }
00478 
00480   const std::vector<std::string>& getAttachedEndEffectorNames() const
00481   {
00482     return attached_end_effector_names_;
00483   }
00484 
00492   bool getEndEffectorTips(std::vector<const LinkModel*>& tips) const;
00493 
00501   bool getEndEffectorTips(std::vector<std::string>& tips) const;
00502 
00508   const moveit::core::LinkModel* getOnlyOneEndEffectorTip() const;
00509 
00511   const JointBoundsVector& getActiveJointModelsBounds() const
00512   {
00513     return active_joint_models_bounds_;
00514   }
00515 
00516   const std::pair<KinematicsSolver, KinematicsSolverMap>& getGroupKinematics() const
00517   {
00518     return group_kinematics_;
00519   }
00520 
00521   void setSolverAllocators(const SolverAllocatorFn& solver,
00522                            const SolverAllocatorMapFn& solver_map = SolverAllocatorMapFn())
00523   {
00524     setSolverAllocators(std::make_pair(solver, solver_map));
00525   }
00526 
00527   void setSolverAllocators(const std::pair<SolverAllocatorFn, SolverAllocatorMapFn>& solvers);
00528 
00529   const kinematics::KinematicsBaseConstPtr& getSolverInstance() const
00530   {
00531     return group_kinematics_.first.solver_instance_const_;
00532   }
00533 
00534   const kinematics::KinematicsBasePtr& getSolverInstance()
00535   {
00536     return group_kinematics_.first.solver_instance_;
00537   }
00538 
00539   bool canSetStateFromIK(const std::string& tip) const;
00540 
00541   bool setRedundantJoints(const std::vector<std::string>& joints)
00542   {
00543     if (group_kinematics_.first.solver_instance_)
00544       return group_kinematics_.first.solver_instance_->setRedundantJoints(joints);
00545     return false;
00546   }
00547 
00549   double getDefaultIKTimeout() const
00550   {
00551     return group_kinematics_.first.default_ik_timeout_;
00552   }
00553 
00555   void setDefaultIKTimeout(double ik_timeout);
00556 
00558   unsigned int getDefaultIKAttempts() const
00559   {
00560     return group_kinematics_.first.default_ik_attempts_;
00561   }
00562 
00564   void setDefaultIKAttempts(unsigned int ik_attempts);
00565 
00570   const std::vector<unsigned int>& getKinematicsSolverJointBijection() const
00571   {
00572     return group_kinematics_.first.bijection_;
00573   }
00574 
00576   void printGroupInfo(std::ostream& out = std::cout) const;
00577 
00578 protected:
00579   bool computeIKIndexBijection(const std::vector<std::string>& ik_jnames,
00580                                std::vector<unsigned int>& joint_bijection) const;
00581 
00585   void updateMimicJoints(double* values) const;
00586 
00588   const RobotModel* parent_model_;
00589 
00591   std::string name_;
00592 
00594   std::vector<const JointModel*> joint_model_vector_;
00595 
00597   std::vector<std::string> joint_model_name_vector_;
00598 
00600   std::vector<const JointModel*> active_joint_model_vector_;
00601 
00603   std::vector<std::string> active_joint_model_name_vector_;
00604 
00606   std::vector<const JointModel*> fixed_joints_;
00607 
00609   std::vector<const JointModel*> mimic_joints_;
00610 
00612   std::vector<const JointModel*> continuous_joint_model_vector_;
00613 
00616   std::vector<std::string> variable_names_;
00617 
00620   std::set<std::string> variable_names_set_;
00621 
00623   JointModelMapConst joint_model_map_;
00624 
00626   std::vector<const JointModel*> joint_roots_;
00627 
00629   const JointModel* common_root_;
00630 
00634   VariableIndexMap joint_variables_index_map_;
00635 
00637   JointBoundsVector active_joint_models_bounds_;
00638 
00641   std::vector<int> variable_index_list_;
00642 
00645   std::vector<int> active_joint_model_start_index_;
00646 
00650   std::vector<const LinkModel*> link_model_vector_;
00651 
00653   LinkModelMapConst link_model_map_;
00654 
00656   std::vector<std::string> link_model_name_vector_;
00657 
00658   std::vector<const LinkModel*> link_model_with_geometry_vector_;
00659 
00661   std::vector<std::string> link_model_with_geometry_name_vector_;
00662 
00665   std::vector<const LinkModel*> updated_link_model_vector_;
00666 
00669   std::set<const LinkModel*> updated_link_model_set_;
00670 
00673   std::vector<std::string> updated_link_model_name_vector_;
00674 
00677   std::set<std::string> updated_link_model_name_set_;
00678 
00681   std::vector<const LinkModel*> updated_link_model_with_geometry_vector_;
00682 
00685   std::set<const LinkModel*> updated_link_model_with_geometry_set_;
00686 
00689   std::vector<std::string> updated_link_model_with_geometry_name_vector_;
00690 
00693   std::set<std::string> updated_link_model_with_geometry_name_set_;
00694 
00696   unsigned int variable_count_;
00697 
00700   bool is_contiguous_index_list_;
00701 
00703   std::vector<std::string> subgroup_names_;
00704 
00706   std::set<std::string> subgroup_names_set_;
00707 
00709   std::vector<std::string> attached_end_effector_names_;
00710 
00714   std::pair<std::string, std::string> end_effector_parent_;
00715 
00717   std::string end_effector_name_;
00718 
00719   bool is_chain_;
00720 
00721   bool is_single_dof_;
00722 
00723   struct GroupMimicUpdate
00724   {
00725     GroupMimicUpdate(int s, int d, double f, double o) : src(s), dest(d), factor(f), offset(o)
00726     {
00727     }
00728     int src;
00729     int dest;
00730     double factor;
00731     double offset;
00732   };
00733 
00734   std::vector<GroupMimicUpdate> group_mimic_update_;
00735 
00736   std::pair<KinematicsSolver, KinematicsSolverMap> group_kinematics_;
00737 
00738   srdf::Model::Group config_;
00739 
00741   std::map<std::string, std::map<std::string, double> > default_states_;
00742 
00744   std::vector<std::string> default_states_names_;
00745 };
00746 }
00747 }
00748 
00749 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:43