00001 /* 00002 * jaco_arm.h 00003 * 00004 * Created on: Mar 6, 2013 00005 * Author: mdedonato 00006 */ 00007 00008 #ifndef JACO_DRIVER_JACO_ARM_KINEMATICS_H 00009 #define JACO_DRIVER_JACO_ARM_KINEMATICS_H 00010 00011 #include <math.h> 00012 00013 #include <ros/ros.h> 00014 #include <tf/tf.h> 00015 #include <tf/transform_broadcaster.h> 00016 00017 #include <string> 00018 00019 00020 /******************************************/ 00021 /**************DH Parameters***************/ 00022 /******************************************/ 00023 /* i * alpha(i-1) * a(i-1) * di * theta1 */ 00024 /******************************************/ 00025 /* 1 * 0 * 0 * D1 * q1 */ 00026 /* 2 * -pi/2 * 0 * 0 * q2 */ 00027 /* 3 * 0 * D2 * 0 * q3 */ 00028 /* 4 * -pi/2 * 0 * d4b * q4 */ 00029 /* 5 * 2*aa * 0 * d5b * q5 */ 00030 /* 6 * 2*aa * 0 * d6b * q6 */ 00031 /******************************************/ 00032 00033 namespace jaco 00034 { 00035 00036 class JacoKinematics 00037 { 00038 public: 00039 explicit JacoKinematics(const ros::NodeHandle& node_handle); 00040 00041 void updateForward(float q1, float q2, float q3, float q4, float q5, float q6); 00042 00043 inline float degToRad(float degrees) 00044 { 00045 return (degrees * (M_PI / 180)); 00046 } 00047 00048 private: 00049 tf::TransformBroadcaster broadcaster_; 00050 std::string tf_prefix_; 00051 00052 /* Robot Length Values (Meters) */ 00053 double base_to_api_; 00054 double base_to_j1_; // base to joint1 (Meters) 00055 double j1_to_j2_; // joint1 to joint2 (Meters) 00056 double j2_to_j3_; // arm length (Meters) 00057 double j3_offset_; // Arm Length (Meters) 00058 double j3_to_j4_; // Front Arm Length (Meters) 00059 double j4_to_j5_; // First Wrist Length (Meters) 00060 double j5_to_j6_; // Second Wrist Length (Meters) 00061 double j6_to_end_; // Wrist to Center of Hand(Meters) 00062 double j5_bend_degrees_; // 00063 double j6_bend_degrees_; 00064 }; 00065 00066 } // namespace jaco 00067 #endif // JACO_DRIVER_JACO_ARM_KINEMATICS_H