00001 #ifndef _WAM_IK_KDL_H_ 00002 #define _WAM_IK_KDL_H_ 00003 00004 #include <math.h> 00005 #include <Eigen/StdVector> 00006 #include <Eigen/Dense> 00007 #include <string> 00008 #include "ros/this_node.h" 00009 #include "ros/names.h" 00010 #include "ros/node_handle.h" 00011 #include "ros/rate.h" 00012 00013 // kdl includes 00014 #include <kdl/chain.hpp> 00015 #include <kdl/chainfksolver.hpp> 00016 #include <kdl/frames_io.hpp> 00017 #include <kdl/chainfksolverpos_recursive.hpp> 00018 #include <kdl/chainiksolverpos_nr_jl.hpp> 00019 #include <kdl/chainiksolverpos_nr.hpp> 00020 #include <kdl/chainiksolvervel_pinv.hpp> 00021 #include <kdl/chainiksolvervel_pinv_givens.hpp> 00022 #include <kdl/chainiksolvervel_wdls.hpp> 00023 00024 // [publisher subscriber headers] 00025 #include "sensor_msgs/JointState.h" 00026 00027 // [service client headers] 00028 #include "iri_wam_common_msgs/pose_move.h" 00029 #include "iri_wam_common_msgs/joints_move.h" 00030 #include "iri_wam_common_msgs/wamInverseKinematics.h" 00031 00032 // [action server client headers] 00033 00034 class WamIkKdl { 00035 private: 00036 // 00037 KDL::Chain wam63_; 00038 unsigned int num_joints_; 00039 std::vector<double> currentjoints; 00040 00041 ros::NodeHandle nh_; 00042 // [publisher attributes] 00043 00044 // [subscriber attributes] 00045 ros::Subscriber joint_states_subscriber; 00046 00047 // [server attributes] 00048 ros::ServiceServer move_in_xyzquat_server; 00049 ros::ServiceServer print_ik_xyzquat_server; 00050 00051 // [client attributes] 00052 ros::ServiceClient move_in_joints_client; 00053 iri_wam_common_msgs::joints_move move_in_joints_srv; 00054 00055 // [methods] 00056 void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg); 00057 bool move_in_xyzquat_Callback(iri_wam_common_msgs::pose_move::Request &req, iri_wam_common_msgs::pose_move::Response &res); 00058 bool print_ik_xyzquat_Callback(iri_wam_common_msgs::wamInverseKinematics::Request &req, iri_wam_common_msgs::wamInverseKinematics::Response &res); 00059 bool ik(std::vector<double> pose, std::vector<double> currentjoints, std::vector<double>& joints); 00060 00061 protected: 00062 00063 public: 00064 WamIkKdl(); 00065 00066 }; 00067 #endif