ur_kinematics: ur_moveit_plugin.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 * Copyright (c) 2014, Georgia Tech
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 Georgia Tech 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: Kelsey Hawkins */
00036 
00037 /* Based on orignal source from Willow Garage. License copied below */
00038 
00039 /*********************************************************************
00040 * Software License Agreement (BSD License)
00041 *
00042 * Copyright (c) 2012, Willow Garage, Inc.
00043 * All rights reserved.
00044 *
00045 * Redistribution and use in source and binary forms, with or without
00046 * modification, are permitted provided that the following conditions
00047 * are met:
00048 *
00049 * * Redistributions of source code must retain the above copyright
00050 * notice, this list of conditions and the following disclaimer.
00051 * * Redistributions in binary form must reproduce the above
00052 * copyright notice, this list of conditions and the following
00053 * disclaimer in the documentation and/or other materials provided
00054 * with the distribution.
00055 * * Neither the name of Willow Garage nor the names of its
00056 * contributors may be used to endorse or promote products derived
00057 * from this software without specific prior written permission.
00058 *
00059 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00060 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00061 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00062 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00063 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00064 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00065 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00066 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00067 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00068 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00069 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00070 * POSSIBILITY OF SUCH DAMAGE.
00071 *********************************************************************/
00072 
00073 /* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
00074 
00075 #ifndef UR_KINEMATICS_PLUGIN_
00076 #define UR_KINEMATICS_PLUGIN_
00077 
00078 // ROS
00079 #include <ros/ros.h>
00080 #include <random_numbers/random_numbers.h>
00081 
00082 // System
00083 #include <boost/shared_ptr.hpp>
00084 
00085 // ROS msgs
00086 #include <geometry_msgs/PoseStamped.h>
00087 #include <moveit_msgs/GetPositionFK.h>
00088 #include <moveit_msgs/GetPositionIK.h>
00089 #include <moveit_msgs/GetKinematicSolverInfo.h>
00090 #include <moveit_msgs/MoveItErrorCodes.h>
00091 
00092 // KDL
00093 #include <kdl/jntarray.hpp>
00094 #include <kdl/chainiksolvervel_pinv.hpp>
00095 #include <kdl/chainiksolverpos_nr_jl.hpp>
00096 #include <kdl/chainfksolverpos_recursive.hpp>
00097 #include <moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp>
00098 #include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
00099 #include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
00100 
00101 // MoveIt!
00102 #include <moveit/kinematics_base/kinematics_base.h>
00103 #include <moveit/robot_model/robot_model.h>
00104 #include <moveit/robot_state/robot_state.h>
00105 
00106 namespace ur_kinematics
00107 {
00111   class URKinematicsPlugin : public kinematics::KinematicsBase
00112   {
00113     public:
00114 
00118     URKinematicsPlugin();
00119 
00120     virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00121                                const std::vector<double> &ik_seed_state,
00122                                std::vector<double> &solution,
00123                                moveit_msgs::MoveItErrorCodes &error_code,
00124                                const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00125 
00126     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00127                                   const std::vector<double> &ik_seed_state,
00128                                   double timeout,
00129                                   std::vector<double> &solution,
00130                                   moveit_msgs::MoveItErrorCodes &error_code,
00131                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00132 
00133     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00134                                   const std::vector<double> &ik_seed_state,
00135                                   double timeout,
00136                                   const std::vector<double> &consistency_limits,
00137                                   std::vector<double> &solution,
00138                                   moveit_msgs::MoveItErrorCodes &error_code,
00139                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00140 
00141     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00142                                   const std::vector<double> &ik_seed_state,
00143                                   double timeout,
00144                                   std::vector<double> &solution,
00145                                   const IKCallbackFn &solution_callback,
00146                                   moveit_msgs::MoveItErrorCodes &error_code,
00147                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00148 
00149     virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00150                                   const std::vector<double> &ik_seed_state,
00151                                   double timeout,
00152                                   const std::vector<double> &consistency_limits,
00153                                   std::vector<double> &solution,
00154                                   const IKCallbackFn &solution_callback,
00155                                   moveit_msgs::MoveItErrorCodes &error_code,
00156                                   const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00157 
00158     virtual bool getPositionFK(const std::vector<std::string> &link_names,
00159                                const std::vector<double> &joint_angles,
00160                                std::vector<geometry_msgs::Pose> &poses) const;
00161 
00162     virtual bool initialize(const std::string &robot_description,
00163                             const std::string &group_name,
00164                             const std::string &base_name,
00165                             const std::string &tip_name,
00166                             double search_discretization);
00167 
00171     const std::vector<std::string>& getJointNames() const;
00172 
00176     const std::vector<std::string>& getLinkNames() const;
00177 
00178   protected:
00179 
00195     bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00196                           const std::vector<double> &ik_seed_state,
00197                           double timeout,
00198                           std::vector<double> &solution,
00199                           const IKCallbackFn &solution_callback,
00200                           moveit_msgs::MoveItErrorCodes &error_code,
00201                           const std::vector<double> &consistency_limits,
00202                           const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00203 
00204     virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00205 
00206   private:
00207 
00208     bool timedOut(const ros::WallTime &start_time, double duration) const;
00209 
00210 
00218     bool checkConsistency(const KDL::JntArray& seed_state,
00219                           const std::vector<double> &consistency_limit,
00220                           const KDL::JntArray& solution) const;
00221 
00222     int getJointIndex(const std::string &name) const;
00223 
00224     int getKDLSegmentIndex(const std::string &name) const;
00225 
00226     void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
00227 
00234     void getRandomConfiguration(const KDL::JntArray& seed_state,
00235                                 const std::vector<double> &consistency_limits,
00236                                 KDL::JntArray &jnt_array,
00237                                 bool lock_redundancy) const;
00238 
00239     bool isRedundantJoint(unsigned int index) const;
00240 
00241     bool active_; 
00243     moveit_msgs::KinematicSolverInfo ik_chain_info_; 
00245     moveit_msgs::KinematicSolverInfo fk_chain_info_; 
00247     KDL::Chain kdl_chain_;
00248 
00249     unsigned int dimension_; 
00251     KDL::JntArray joint_min_, joint_max_; 
00253     mutable random_numbers::RandomNumberGenerator random_number_generator_;
00254 
00255     robot_model::RobotModelPtr robot_model_;
00256 
00257     robot_state::RobotStatePtr state_, state_2_;
00258 
00259     int num_possible_redundant_joints_;
00260     std::vector<unsigned int> redundant_joints_map_index_;
00261 
00262     // Storage required for when the set of redundant joints is reset
00263     bool position_ik_; //whether this solver is only being used for position ik
00264     robot_model::JointModelGroup* joint_model_group_;
00265     double max_solver_iterations_;
00266     double epsilon_;
00267     std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
00268 
00269     std::vector<double> ik_weights_;
00270     std::vector<std::string> ur_joint_names_;
00271     std::vector<std::string> ur_link_names_;
00272     int ur_joint_inds_start_;
00273     std::string arm_prefix_;
00274 
00275     // kinematic chains representing the chain from the group base link to the
00276     // UR base link, and the UR tip link to the group tip link
00277     KDL::Chain kdl_base_chain_;
00278     KDL::Chain kdl_tip_chain_;
00279 
00280   };
00281 }
00282 
00283 #endif