kinematics_base.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta, Dave Coleman */
00036 
00037 #ifndef MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
00038 #define MOVEIT_KINEMATICS_BASE_KINEMATICS_BASE_
00039 
00040 #include <geometry_msgs/PoseStamped.h>
00041 #include <moveit_msgs/MoveItErrorCodes.h>
00042 #include <moveit/macros/class_forward.h>
00043 #include <ros/node_handle.h>
00044 #include <console_bridge/console.h>
00045 
00046 #include <boost/function.hpp>
00047 #include <string>
00048 
00049 namespace moveit
00050 {
00051 namespace core
00052 {
00053 class JointModelGroup;
00054 class RobotState;
00055 }
00056 }
00057 
00059 namespace kinematics
00060 {
00061 /*
00062  * @enum DiscretizationMethods
00063  *
00064  * @brief Flags for choosing the type discretization method applied on the redundant joints during an ik query
00065  */
00066 namespace DiscretizationMethods
00067 {
00068 enum DiscretizationMethod
00069 {
00070   NO_DISCRETIZATION = 1, 
00071   ALL_DISCRETIZED,       
00072   SOME_DISCRETIZED, 
00075   ALL_RANDOM_SAMPLED, 
00076   SOME_RANDOM_SAMPLED 
00078 };
00079 }
00080 typedef DiscretizationMethods::DiscretizationMethod DiscretizationMethod;
00081 
00082 /*
00083  * @enum KinematicErrors
00084  * @brief Kinematic error codes that occur in a ik query
00085  */
00086 namespace KinematicErrors
00087 {
00088 enum KinematicError
00089 {
00090   OK = 1,                              
00091   UNSUPORTED_DISCRETIZATION_REQUESTED, 
00092   DISCRETIZATION_NOT_INITIALIZED,      
00094   MULTIPLE_TIPS_NOT_SUPPORTED,         
00095   EMPTY_TIP_POSES,                     
00096   IK_SEED_OUTSIDE_LIMITS,              
00097   SOLVER_NOT_ACTIVE,                   
00098   NO_SOLUTION                          
00100 };
00101 }
00102 typedef KinematicErrors::KinematicError KinematicError;
00103 
00108 struct KinematicsQueryOptions
00109 {
00110   KinematicsQueryOptions()
00111     : lock_redundant_joints(false)
00112     , return_approximate_solution(false)
00113     , discretization_method(DiscretizationMethods::NO_DISCRETIZATION)
00114   {
00115   }
00116 
00117   bool lock_redundant_joints;                 
00118   bool return_approximate_solution;           
00119   DiscretizationMethod discretization_method; 
00121 };
00122 
00123 /*
00124  * @struct KinematicsResult
00125  * @brief Reports result details of an ik query
00126  *
00127  * This struct is used as an output argument of the getPositionIK(...) method that returns multiple joint solutions.
00128  * It contains the type of error that led to a failure or KinematicErrors::OK when a set of joint solutions is found.
00129  * The solution percentage shall provide a ration of solutions found over solutions searched.
00130  *
00131  */
00132 struct KinematicsResult
00133 {
00134   KinematicError kinematic_error; 
00135   double solution_percentage;     
00137 };
00138 
00139 MOVEIT_CLASS_FORWARD(KinematicsBase);
00140 
00145 class KinematicsBase
00146 {
00147 public:
00148   static const double DEFAULT_SEARCH_DISCRETIZATION; /* = 0.1 */
00149   static const double DEFAULT_TIMEOUT;               /* = 1.0 */
00150 
00152   typedef boost::function<void(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_solution,
00153                                moveit_msgs::MoveItErrorCodes& error_code)>
00154       IKCallbackFn;
00155 
00166   virtual bool
00167   getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00168                 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00169                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00170 
00186   virtual bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
00187                              std::vector<std::vector<double> >& solutions, KinematicsResult& result,
00188                              const kinematics::KinematicsQueryOptions& options) const;
00189 
00203   virtual bool
00204   searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00205                    std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00206                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00207 
00223   virtual bool
00224   searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00225                    const std::vector<double>& consistency_limits, std::vector<double>& solution,
00226                    moveit_msgs::MoveItErrorCodes& error_code,
00227                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00228 
00243   virtual bool searchPositionIK(
00244       const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00245       std::vector<double>& solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00246       const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00247 
00264   virtual bool
00265   searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00266                    const std::vector<double>& consistency_limits, std::vector<double>& solution,
00267                    const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00268                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const = 0;
00269 
00295   virtual bool
00296   searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
00297                    double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
00298                    const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00299                    const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
00300                    const moveit::core::RobotState* context_state = NULL) const
00301   {
00302     // For IK solvers that do not support multiple poses, fall back to single pose call
00303     if (ik_poses.size() == 1)
00304     {
00305       // Check if a solution_callback function was provided and call the corresponding function
00306       if (solution_callback)
00307       {
00308         return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, solution_callback,
00309                                 error_code, options);
00310       }
00311       else
00312       {
00313         return searchPositionIK(ik_poses[0], ik_seed_state, timeout, consistency_limits, solution, error_code, options);
00314       }
00315     }
00316 
00317     // Otherwise throw error because this function should have been implemented
00318     logError("moveit.kinematics_base: This kinematic solver does not support searchPositionIK with multiple poses");
00319     return false;
00320   }
00321 
00329   virtual bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
00330                              std::vector<geometry_msgs::Pose>& poses) const = 0;
00331 
00342   virtual void setValues(const std::string& robot_description, const std::string& group_name,
00343                          const std::string& base_frame, const std::string& tip_frame, double search_discretization);
00344 
00355   virtual void setValues(const std::string& robot_description, const std::string& group_name,
00356                          const std::string& base_frame, const std::vector<std::string>& tip_frames,
00357                          double search_discretization);
00358 
00370   virtual bool initialize(const std::string& robot_description, const std::string& group_name,
00371                           const std::string& base_frame, const std::string& tip_frame,
00372                           double search_discretization) = 0;
00373 
00385   virtual bool initialize(const std::string& robot_description, const std::string& group_name,
00386                           const std::string& base_frame, const std::vector<std::string>& tip_frames,
00387                           double search_discretization)
00388   {
00389     // For IK solvers that do not support multiple tip frames, fall back to single pose call
00390     if (tip_frames.size() == 1)
00391     {
00392       return initialize(robot_description, group_name, base_frame, tip_frames[0], search_discretization);
00393     }
00394 
00395     logError("moveit.kinematics_base: This kinematic solver does not support initialization with more than one tip "
00396              "frames");
00397     return false;
00398   }
00399 
00404   virtual const std::string& getGroupName() const
00405   {
00406     return group_name_;
00407   }
00408 
00414   virtual const std::string& getBaseFrame() const
00415   {
00416     return base_frame_;
00417   }
00418 
00426   virtual const std::string& getTipFrame() const
00427   {
00428     if (tip_frames_.size() > 1)
00429       logError("moveit.kinematics_base: This kinematic solver has more than one tip frame, do not call getTipFrame()");
00430 
00431     return tip_frame_;  // for backwards-compatibility. should actually use tip_frames_[0]
00432   }
00433 
00439   virtual const std::vector<std::string>& getTipFrames() const
00440   {
00441     return tip_frames_;
00442   }
00443 
00453   virtual bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
00454 
00462   bool setRedundantJoints(const std::vector<std::string>& redundant_joint_names);
00463 
00467   virtual void getRedundantJoints(std::vector<unsigned int>& redundant_joint_indices) const
00468   {
00469     redundant_joint_indices = redundant_joint_indices_;
00470   }
00471 
00475   virtual const std::vector<std::string>& getJointNames() const = 0;
00476 
00480   virtual const std::vector<std::string>& getLinkNames() const = 0;
00481 
00498   virtual bool supportsGroup(const moveit::core::JointModelGroup* jmg, std::string* error_text_out = NULL) const;
00499 
00503   void setSearchDiscretization(double sd)
00504   {
00505     redundant_joint_discretization_.clear();
00506     for (std::vector<unsigned int>::iterator i = redundant_joint_indices_.begin(); i != redundant_joint_indices_.end();
00507          i++)
00508     {
00509       redundant_joint_discretization_[*i] = sd;
00510     }
00511   }
00512 
00520   void setSearchDiscretization(const std::map<int, double>& discretization)
00521   {
00522     redundant_joint_discretization_.clear();
00523     redundant_joint_indices_.clear();
00524     for (std::map<int, double>::const_iterator i = discretization.begin(); i != discretization.end(); i++)
00525     {
00526       redundant_joint_discretization_.insert(*i);
00527       redundant_joint_indices_.push_back(i->first);
00528     }
00529   }
00530 
00534   double getSearchDiscretization(int joint_index = 0) const
00535   {
00536     if (redundant_joint_discretization_.count(joint_index) > 0)
00537     {
00538       return redundant_joint_discretization_.at(joint_index);
00539     }
00540     else
00541     {
00542       return 0.0;  // returned when there aren't any redundant joints
00543     }
00544   }
00545 
00550   std::vector<DiscretizationMethod> getSupportedDiscretizationMethods() const
00551   {
00552     return supported_methods_;
00553   }
00554 
00557   void setDefaultTimeout(double timeout)
00558   {
00559     default_timeout_ = timeout;
00560   }
00561 
00564   double getDefaultTimeout() const
00565   {
00566     return default_timeout_;
00567   }
00568 
00572   virtual ~KinematicsBase()
00573   {
00574   }
00575 
00576   KinematicsBase()
00577     : tip_frame_("DEPRECATED")
00578     ,  // help users understand why this variable might not be set
00579        // (if multiple tip frames provided, this variable will be unset)
00580     search_discretization_(DEFAULT_SEARCH_DISCRETIZATION)
00581     , default_timeout_(DEFAULT_TIMEOUT)
00582   {
00583     supported_methods_.push_back(DiscretizationMethods::NO_DISCRETIZATION);
00584   }
00585 
00586 protected:
00587   std::string robot_description_;
00588   std::string group_name_;
00589   std::string base_frame_;
00590   std::vector<std::string> tip_frames_;
00591   std::string tip_frame_;  // DEPRECATED - this variable only still exists for backwards compatibility with
00592                            // previously generated custom ik solvers like IKFast
00593 
00594   double search_discretization_;  // DEPRECATED - this variable only still exists for backwards compatibility
00595                                   // with previous implementations.  Discretization values for each joint are
00596                                   // now stored in the redundant_joint_discretization_ member
00597 
00598   double default_timeout_;
00599   std::vector<unsigned int> redundant_joint_indices_;
00600   std::map<int, double> redundant_joint_discretization_;
00601   std::vector<DiscretizationMethod> supported_methods_;
00602 
00612   template <typename T>
00613   inline bool lookupParam(const std::string& param, T& val, const T& default_val) const
00614   {
00615     ros::NodeHandle pnh("~");
00616     if (pnh.hasParam(param))
00617     {
00618       val = pnh.param(param, default_val);
00619       return true;
00620     }
00621 
00622     ros::NodeHandle nh;
00623     if (nh.hasParam("robot_description_kinematics/" + group_name_ + "/" + param))
00624     {
00625       val = nh.param("robot_description_kinematics/" + group_name_ + "/" + param, default_val);
00626       return true;
00627     }
00628 
00629     if (nh.hasParam("robot_description_kinematics/" + param))
00630     {
00631       val = nh.param("robot_description_kinematics/" + param, default_val);
00632       return true;
00633     }
00634 
00635     val = default_val;
00636 
00637     return false;
00638   }
00639 
00640 private:
00641   std::string removeSlash(const std::string& str) const;
00642 };
00643 };
00644 
00645 #endif


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