00001 00011 #ifndef JACO_ARM_KINEMATICS_H_ 00012 #define JACO_ARM_KINEMATICS_H_ 00013 00014 #include <ros/ros.h> 00015 #include <wpi_jaco_msgs/JacoFK.h> 00016 #include <tf/tf.h> 00017 00018 //Link lengths and offsets (JACO) 00019 #define D1 .2755 00020 #define D2 .4100 00021 #define D3 .2073 00022 #define D4 .0743 00023 #define D5 .0743 00024 #define D6 .1687 00025 #define E2 .0098 00026 00027 //Link lengths and offsets (JACO2) 00028 #define J2D1 .2755 00029 #define J2D2 .4100 00030 #define J2D3 .2073 00031 #define J2D4 .0741 00032 #define J2D5 .0741 00033 #define J2D6 .1600 00034 #define J2E2 .0098 00035 00036 #define PI 3.14159 00037 00045 class JacoKinematics 00046 { 00047 00048 public: 00049 00050 JacoKinematics(void); 00051 00058 bool callFK(wpi_jaco_msgs::JacoFK::Request &req, wpi_jaco_msgs::JacoFK::Response &res); 00059 00065 geometry_msgs::PoseStamped calculateFK(std::vector<float> joints); 00066 00075 tf::Transform generateTransform(float theta, float d, float a, float alpha); 00076 00077 private: 00078 bool loadParameters(const ros::NodeHandle n); 00079 00080 ros::NodeHandle n; 00081 ros::ServiceServer fkServer; 00082 00083 std::string arm_name_; 00084 std::string topic_prefix_; 00085 00086 //robot parameters 00087 std::vector<double> ds; 00088 std::vector<double> as; 00089 std::vector<double> alphas; 00090 }; 00091 00092 #endif