Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef KDL_ARM_KINEMATICS_PLUGIN_H_
00039 #define KDL_ARM_KINEMATICS_PLUGIN_H_
00040
00041
00042 #include <ros/ros.h>
00043 #include <tf/tf.h>
00044 #include <tf/transform_listener.h>
00045 #include <tf/transform_datatypes.h>
00046 #include <tf_conversions/tf_kdl.h>
00047
00048
00049 #include <boost/shared_ptr.hpp>
00050 #include <algorithm>
00051 #include <numeric>
00052 #include <cstring>
00053
00054
00055 #include <geometry_msgs/PoseStamped.h>
00056 #include <kinematics_msgs/GetPositionFK.h>
00057 #include <kinematics_msgs/GetPositionIK.h>
00058 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00059 #include <motion_planning_msgs/ArmNavigationErrorCodes.h>
00060
00061
00062 #include <kinematics_base/kinematics_base.h>
00063
00064
00065 #include <kdl/jntarray.hpp>
00066 #include <kdl_parser/kdl_parser.hpp>
00067 #include <kdl/chainiksolverpos_nr_jl.hpp>
00068 #include <kdl/chainiksolvervel_pinv.hpp>
00069 #include <kdl/chainfksolverpos_recursive.hpp>
00070
00071
00072 #include <angles/angles.h>
00073
00074 namespace arm_kinematics_constraint_aware
00075 {
00076 class KDLArmKinematicsPlugin : public kinematics::KinematicsBase
00077 {
00078 public:
00079
00083 KDLArmKinematicsPlugin();
00084
00089 bool isActive();
00090
00098 bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00099 const std::vector<double> &ik_seed_state,
00100 std::vector<double> &solution,
00101 int &error_code);
00102
00111 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00112 const std::vector<double> &ik_seed_state,
00113 const double &timeout,
00114 std::vector<double> &solution,
00115 int &error_code);
00124 bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00125 const std::vector<double> &ik_seed_state,
00126 const double &timeout,
00127 std::vector<double> &solution,
00128 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00129 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00130 int &error_code);
00137 bool getPositionFK(const std::vector<std::string> &link_names,
00138 const std::vector<double> &joint_angles,
00139 std::vector<geometry_msgs::Pose> &poses);
00140
00145 bool initialize(std::string name);
00146
00151 std::string getBaseFrame();
00152
00156 std::string getToolFrame();
00157
00161 std::vector<std::string> getJointNames();
00162
00166 std::vector<std::string> getLinkNames();
00167
00168 protected:
00169
00170 bool loadModel(const std::string xml);
00171 bool readJoints(urdf::Model &robot_model);
00172 int getJointIndex(const std::string &name);
00173 int getKDLSegmentIndex(const std::string &name);
00174 double genRandomNumber(const double &min, const double &max);
00175 KDL::JntArray getRandomConfiguration();
00176
00177 int max_search_iterations_;
00178
00179 bool active_;
00180 tf::TransformListener tf_;
00181 kinematics_msgs::KinematicSolverInfo chain_info_;
00182
00183 boost::shared_ptr<KDL::ChainIkSolverVel_pinv> ik_solver_vel_;
00184 boost::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_;
00185 boost::shared_ptr<KDL::ChainIkSolverPos_NR_JL> ik_solver_pos_;
00186
00187 unsigned int dimension_;
00188 KDL::Chain kdl_chain_;
00189 std::string root_name_,tip_name_;
00190 KDL::JntArray joint_min_, joint_max_;
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204 };
00205 }
00206
00207 #endif