kdl_kinematics_plugin.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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, David Lu!!, Ugo Cupcic */
00036 
00037 #ifndef MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
00038 #define MOVEIT_ROS_PLANNING_KDL_KINEMATICS_PLUGIN_
00039 
00040 // ROS
00041 #include <ros/ros.h>
00042 #include <random_numbers/random_numbers.h>
00043 
00044 // System
00045 #include <boost/shared_ptr.hpp>
00046 
00047 // ROS msgs
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <moveit_msgs/GetPositionFK.h>
00050 #include <moveit_msgs/GetPositionIK.h>
00051 #include <moveit_msgs/GetKinematicSolverInfo.h>
00052 #include <moveit_msgs/MoveItErrorCodes.h>
00053 
00054 // KDL
00055 #include <kdl/jntarray.hpp>
00056 #include <kdl/chainiksolvervel_pinv.hpp>
00057 #include <kdl/chainiksolverpos_nr_jl.hpp>
00058 #include <kdl/chainfksolverpos_recursive.hpp>
00059 #include <moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp>
00060 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00061 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00062 
00063 // MoveIt!
00064 #include <moveit/kinematics_base/kinematics_base.h>
00065 #include <moveit/robot_model/robot_model.h>
00066 #include <moveit/robot_state/robot_state.h>
00067 
00068 namespace kdl_kinematics_plugin
00069 {
00073   class KDLKinematicsPlugin : public kinematics::KinematicsBase
00074   {
00075     public:
00076 
00080     KDLKinematicsPlugin();
00081 
00082     virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00083                                const std::vector<double> &ik_seed_state,
00084                                std::vector<double> &solution,
00085                                moveit_msgs::MoveItErrorCodes &error_code,
00086                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00087 
00088     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00089                                   const std::vector<double> &ik_seed_state,
00090                                   double timeout,
00091                                   std::vector<double> &solution,
00092                                   moveit_msgs::MoveItErrorCodes &error_code,
00093                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00094 
00095     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00096                                   const std::vector<double> &ik_seed_state,
00097                                   double timeout,
00098                                   const std::vector<double> &consistency_limits,
00099                                   std::vector<double> &solution,
00100                                   moveit_msgs::MoveItErrorCodes &error_code,
00101                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00102 
00103     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00104                                   const std::vector<double> &ik_seed_state,
00105                                   double timeout,
00106                                   std::vector<double> &solution,
00107                                   const IKCallbackFn &solution_callback,
00108                                   moveit_msgs::MoveItErrorCodes &error_code,
00109                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00110 
00111     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00112                                   const std::vector<double> &ik_seed_state,
00113                                   double timeout,
00114                                   const std::vector<double> &consistency_limits,
00115                                   std::vector<double> &solution,
00116                                   const IKCallbackFn &solution_callback,
00117                                   moveit_msgs::MoveItErrorCodes &error_code,
00118                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00119 
00120     virtual bool getPositionFK(const std::vector<std::string> &link_names,
00121                                const std::vector<double> &joint_angles,
00122                                std::vector<geometry_msgs::Pose> &poses) const;
00123 
00124     virtual bool initialize(const std::string &robot_description,
00125                             const std::string &group_name,
00126                             const std::string &base_name,
00127                             const std::string &tip_name,
00128                             double search_discretization);
00129 
00133     const std::vector<std::string>& getJointNames() const;
00134 
00138     const std::vector<std::string>& getLinkNames() const;
00139 
00140   protected:
00141 
00157     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00158                           const std::vector<double> &ik_seed_state,
00159                           double timeout,
00160                           std::vector<double> &solution,
00161                           const IKCallbackFn &solution_callback,
00162                           moveit_msgs::MoveItErrorCodes &error_code,
00163                           const std::vector<double> &consistency_limits,
00164                           const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00165 
00166     virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00167 
00168   private:
00169 
00170     bool timedOut(const ros::WallTime &start_time, double duration) const;
00171 
00172 
00180     bool checkConsistency(const KDL::JntArray& seed_state,
00181                           const std::vector<double> &consistency_limit,
00182                           const KDL::JntArray& solution) const;
00183 
00184     int getJointIndex(const std::string &name) const;
00185 
00186     int getKDLSegmentIndex(const std::string &name) const;
00187 
00188     void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
00189 
00196     void getRandomConfiguration(const KDL::JntArray& seed_state,
00197                                 const std::vector<double> &consistency_limits,
00198                                 KDL::JntArray &jnt_array,
00199                                 bool lock_redundancy) const;
00200 
00201     bool isRedundantJoint(unsigned int index) const;
00202 
00203     bool active_; 
00205     moveit_msgs::KinematicSolverInfo ik_chain_info_; 
00207     moveit_msgs::KinematicSolverInfo fk_chain_info_; 
00209     KDL::Chain kdl_chain_;
00210 
00211     unsigned int dimension_; 
00213     KDL::JntArray joint_min_, joint_max_; 
00215     mutable random_numbers::RandomNumberGenerator random_number_generator_;
00216 
00217     robot_model::RobotModelPtr robot_model_;
00218 
00219     robot_state::RobotStatePtr state_, state_2_;
00220 
00221     int num_possible_redundant_joints_;
00222     std::vector<unsigned int> redundant_joints_map_index_;
00223 
00224     // Storage required for when the set of redundant joints is reset
00225     bool position_ik_; //whether this solver is only being used for position ik
00226     robot_model::JointModelGroup* joint_model_group_;
00227     double max_solver_iterations_;
00228     double epsilon_;
00229     std::vector<JointMimic> mimic_joints_;
00230 
00231   };
00232 }
00233 
00234 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Aug 26 2015 12:43:30