jaco_arm_kinematics.cpp
Go to the documentation of this file.
00001 /*
00002  * jacolib.cpp
00003  *
00004  *  Created on: Mar 10, 2013
00005  *      Author: mdedonato
00006  */
00007 
00008 #include <jaco_driver/jaco_arm_kinematics.h>
00009 #include <string>
00010 
00011 
00012 namespace jaco
00013 {
00014 
00015 std::string concatTfName(const std::string& prefix, const std::string name)
00016 {
00017     std::stringstream ss;
00018     ss << prefix << name;
00019     return ss.str();
00020 }
00021 
00022 
00023 JacoKinematics::JacoKinematics(const ros::NodeHandle &node_handle)
00024 {
00025     node_handle.param<std::string>("tf_prefix", tf_prefix_, "jaco_");
00026 
00027     node_handle.param<double>("base_to_api", base_to_api_, 0.028);
00028     node_handle.param<double>("base_to_j1", base_to_j1_, 0.1544);
00029     node_handle.param<double>("j1_to_j2", j1_to_j2_, -0.1181);
00030     node_handle.param<double>("j2_to_j3", j2_to_j3_, 0.4100);
00031     node_handle.param<double>("j3_offset", j3_offset_, -0.0098);
00032     node_handle.param<double>("j3_to_j4", j3_to_j4_, 0.2073);
00033     node_handle.param<double>("j4_to_j5", j4_to_j5_, 0.0743);
00034     node_handle.param<double>("j5_to_j6", j5_to_j6_, 0.0743);
00035     node_handle.param<double>("j6_to_end", j6_to_end_, 0.1687);
00036     node_handle.param<double>("j5_bend_degrees", j5_bend_degrees_, -55.0);
00037     node_handle.param<double>("j6_bend_degrees", j6_bend_degrees_, 55.0);
00038 }
00039 
00040 
00041 void JacoKinematics::updateForward(float q1, float q2, float q3, float q4, float q5, float q6)
00042 {
00043     tf::Transform transform;
00044     tf::Quaternion rotation_q(0, 0, 0, 1);
00045     tf::Matrix3x3 rot_matrix(1, 0, 0, 0, 1, 0, 0, 0, 1);
00046     tf::Vector3 translation_v(0, 0, 0);
00047 
00048     /**********************Base**********************/
00049     /* Rotation Matrix */
00050     /*******               *******
00051     * cos(PI/2)  -sin(PI/2)    0 *
00052     * sin(PI/2)   cos(PI/2)    0 *
00053     * 0              0         1 *
00054     *******               *******/
00055     rot_matrix.setValue(cos(M_PI_2), -sin(M_PI_2), 0,
00056                         sin(M_PI_2), cos(M_PI_2), 0,
00057                         0, 0, 1);
00058     rot_matrix.getRotation(rotation_q);
00059     transform.setRotation(rotation_q);
00060 
00061     /* Translation Vector */
00062     translation_v.setValue(0, 0, 0);
00063     transform.setOrigin(translation_v);
00064 
00065     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00066                                                     concatTfName(tf_prefix_, "arm_base"),
00067                                                     concatTfName(tf_prefix_, "base")));
00068 
00069 #ifdef PRINT_DEBUG_INFO
00070     ROS_INFO("API Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation
00071              _q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00072 #endif
00073 
00074     /**********************API**********************/
00075     /* Rotation Matrix */
00076     rot_matrix.setValue(1, 0, 0,
00077                         0, 1, 0,
00078                         0, 0, 1);
00079     rot_matrix.getRotation(rotation_q);
00080     transform.setRotation(rotation_q);
00081 
00082     /* API Translation Vector */
00083     translation_v.setValue(0, 0, base_to_api_);
00084     transform.setOrigin(translation_v);
00085 
00086     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00087                                                     concatTfName(tf_prefix_, "base"),
00088                                                     concatTfName(tf_prefix_, "api_origin")));
00089 
00090 #ifdef PRINT_DEBUG_INFO
00091     ROS_INFO("API Translation: X = %f, Y = %f, Z = %f",
00092              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00093 #endif
00094 
00095     /**********************Joint_1**********************/
00096     /* Joint 1 Rotation Matrix */
00097     /*******                 *******
00098      * cos(q1)    -sin(q1)      0  *
00099      * -sin(q1)   -cos(q1)      0  *
00100      * 0            0          -1  *
00101      *******                *******/
00102     rot_matrix.setValue(cos(q1), -sin(q1), 0,
00103                         -sin(q1), -cos(q1), 0,
00104                         0, 0, -1);
00105     rot_matrix.getRotation(rotation_q);
00106     transform.setRotation(rotation_q);
00107 
00108     /* Joint 1 Translation Vector */
00109     translation_v.setValue(0, 0, base_to_j1_);
00110     transform.setOrigin(translation_v);
00111 
00112     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00113                                                     concatTfName(tf_prefix_, "base"),
00114                                                     concatTfName(tf_prefix_, "joint_1")));
00115 
00116 #ifdef PRINT_DEBUG_INFO
00117     ROS_INFO("Joint 1 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00118              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00119     ROS_INFO("Joint 1 Translation: X = %f, Y = %f, Z = %f",
00120              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00121 #endif
00122 
00123     /**********************Joint_2**********************/
00124     /* Joint 2 Rotation Matrix */
00125     /*******               *******
00126      * sin(q2)    cos(q2)     0  *
00127      * 0            0         1  *
00128      * cos(q2)   -sin(q2)     0  *
00129      *******              *******/
00130     rot_matrix.setValue(sin(q2), cos(q2), 0,
00131                         0, 0, 1,
00132                         cos(q2), -sin(q2), 0);
00133     rot_matrix.getRotation(rotation_q);
00134     transform.setRotation(rotation_q);
00135 
00136     /* Joint 2 Translation Vector */
00137     translation_v.setValue(0, 0 , j1_to_j2_);
00138     transform.setOrigin(translation_v);
00139 
00140     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00141                                                     concatTfName(tf_prefix_, "joint_1"),
00142                                                     concatTfName(tf_prefix_, "joint_2")));
00143 
00144 #ifdef PRINT_DEBUG_INFO
00145     ROS_INFO("Joint 2 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00146              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00147     ROS_INFO("Joint 2 Translation: X = %f, Y = %f, Z = %f",
00148              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00149 #endif
00150 
00151     /**********************Joint_3**********************/
00152     /* Joint 3 Rotation Matrix */
00153     /*******                   *******
00154      * -cos(q3)     sin(q3)        0 *
00155      *  sin(q2)     cos(q3)        0 *
00156      *    0           0           -1 *
00157      *******                  *******/
00158     rot_matrix.setValue(-cos(q3), sin(q3), 0,
00159                         sin(q3), cos(q3), 0,
00160                         0, 0, -1);
00161     rot_matrix.getRotation(rotation_q);
00162     transform.setRotation(rotation_q);
00163 
00164     /* Joint 3 Translation Vector */
00165     translation_v.setValue(j2_to_j3_, 0, 0);
00166     transform.setOrigin(translation_v);
00167 
00168     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00169                                                     concatTfName(tf_prefix_, "joint_2"),
00170                                                     concatTfName(tf_prefix_, "joint_3")));
00171 
00172 #ifdef PRINT_DEBUG_INFO
00173     ROS_INFO("Joint 3 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00174              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00175     ROS_INFO("Joint 3 Translation: X = %f, Y = %f, Z = %f",
00176              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00177 #endif
00178 
00179     /**********************Joint_3 Offset**********************/
00180     /* Joint 3 offset Rotation Matrix */
00181     rot_matrix.setValue(1, 0, 0, 0, 1, 0, 0, 0, 1);
00182     rot_matrix.getRotation(rotation_q);
00183     transform.setRotation(rotation_q);
00184 
00185     /* Joint 3 offset translation vector */
00186     translation_v.setValue(0, 0, j3_offset_);
00187     transform.setOrigin(translation_v);
00188 
00189     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00190                                                     concatTfName(tf_prefix_, "joint_3"),
00191                                                     concatTfName(tf_prefix_, "joint_3_offset")));
00192 
00193 #ifdef PRINT_DEBUG_INFO
00194     ROS_INFO("Joint 3 Offset Rotation: X = %f, Y = %f, Z = %f, W = %f",
00195              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00196     ROS_INFO("Joint 3 Offset Translation: X = %f, Y = %f, Z = %f",
00197              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00198 #endif
00199 
00200     /**********************Joint_4**********************/
00201     /* Joint 4 Rotation Matrix */
00202     /*******                 *******
00203      *    0          0          -1 *
00204      * sin(q4)     cos(q4)       0 *
00205      * cos(q4)    -sin(q4)       0 *
00206      *******                *******/
00207     rot_matrix.setValue(0, 0, -1,
00208                         sin(q4), cos(q4), 0,
00209                         cos(q4), -sin(q4), 0);
00210     rot_matrix.getRotation(rotation_q);
00211     transform.setRotation(rotation_q);
00212 
00213     /* Joint 4 Translation Vector */
00214     translation_v.setValue(j3_to_j4_, 0, 0);
00215     transform.setOrigin(translation_v);
00216 
00217     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00218                                                     concatTfName(tf_prefix_, "joint_3_offset"),
00219                                                     concatTfName(tf_prefix_, "joint_4")));
00220 
00221 #ifdef PRINT_DEBUG_INFO
00222     ROS_INFO("Joint 4 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00223              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00224     ROS_INFO("Joint 4 Translation: X = %f, Y = %f, Z = %f",
00225              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00226 #endif
00227 
00228     /**********************Joint_5**********************/
00229     /* Joint 5 Rotation Matrix */
00230     /*******                                         *******
00231      * cos(-55))*cos(q5)    cos(-55)*-sin(q5)     sin(-55) *
00232      * sin(q5)                   cos(q5)             0     *
00233      * -sin(-55)*cos(q5)    sin(-55)*sin(q5)      cos(-55) *
00234      *******                                        *******/
00235     rot_matrix.setValue(cos((degToRad(j5_bend_degrees_)))*cos(q5),
00236                         cos((degToRad(j5_bend_degrees_)))*-sin(q5),
00237                         sin((degToRad(j5_bend_degrees_))),
00238                         sin(q5),
00239                         cos(q5),
00240                         0,
00241                         -sin((degToRad(j5_bend_degrees_)))*cos(q5),
00242                         sin((degToRad(j5_bend_degrees_)))*sin(q5),
00243                         cos((degToRad(j5_bend_degrees_))));
00244     rot_matrix.getRotation(rotation_q);
00245     transform.setRotation(rotation_q);
00246 
00247     /* Joint 5 Translation Vector */
00248     /****        ****
00249      * cos(55)*D4   *
00250      *       0      *
00251      * -sin(55)*D4  *
00252      ****       ****/
00253     translation_v.setValue(cos(degToRad(-j5_bend_degrees_)) * j4_to_j5_,
00254                            0,
00255                            -sin(degToRad(-j5_bend_degrees_)) * j4_to_j5_);
00256     transform.setOrigin(translation_v);
00257 
00258     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00259                                                     concatTfName(tf_prefix_, "joint_4"),
00260                                                     concatTfName(tf_prefix_, "joint_5")));
00261 
00262 #ifdef PRINT_DEBUG_INFO
00263     ROS_INFO("Joint 5 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00264              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00265     ROS_INFO("Joint 5 Translation: X = %f, Y = %f, Z = %f",
00266              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00267 #endif
00268 
00269     /**********************Joint_6**********************/
00270     /* Joint 6 Rotation Matrix */
00271     /*******                                       *******
00272      * cos(-35))*cos(q6)   cos(-35)*-sin(q6)    sin(-35) *
00273      *     sin(q6)              cos(q6)            0     *
00274      * -sin(-35)*cos(q6)   sin(-35)*sin(q6)     cos(-35) *
00275      *******                                       *******/
00276     rot_matrix.setValue(cos((degToRad(j6_bend_degrees_))) * cos(q6),
00277                         cos((degToRad(j6_bend_degrees_))) * -sin(q6),
00278                         sin((degToRad(j6_bend_degrees_))),
00279                         sin(q6),
00280                         cos(q6),
00281                         0,
00282                         -sin((degToRad(j6_bend_degrees_))) * cos(q6),
00283                         sin((degToRad(j6_bend_degrees_))) * sin(q6),
00284                         cos((degToRad(j6_bend_degrees_))));
00285     rot_matrix.getRotation(rotation_q);
00286     transform.setRotation(rotation_q);
00287     /* Joint 6 Translation Vector */
00288     /****        ****
00289      * -cos(55)*D5  *
00290      *       0      *
00291      * -sin(55)*D5  *
00292      ****       ****/
00293     translation_v.setValue(-cos(degToRad(j6_bend_degrees_)) * j5_to_j6_,
00294                            0,
00295                            -sin(degToRad(j6_bend_degrees_)) * j5_to_j6_);
00296     transform.setOrigin(translation_v);
00297 
00298     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00299                                                     concatTfName(tf_prefix_, "joint_5"),
00300                                                     concatTfName(tf_prefix_, "joint_6")));
00301 
00302 #ifdef PRINT_DEBUG_INFO
00303     ROS_INFO("Joint 6 Rotation: X = %f, Y = %f, Z = %f, W = %f",
00304              rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00305     ROS_INFO("Joint 6 Translation: X = %f, Y = %f, Z = %f",
00306              translation_v.getX(), translation_v.getY(), translation_v.getZ());
00307 #endif
00308 
00309     /**********************end effector**********************/
00310     rot_matrix.setValue(1, 0, 0, 0, 1, 0, 0, 0, 1);
00311     rot_matrix.getRotation(rotation_q);
00312     transform.setRotation(rotation_q);
00313 
00314     translation_v.setValue(0, 0, -j6_to_end_);
00315     transform.setOrigin(translation_v);
00316 
00317     broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
00318                                                     concatTfName(tf_prefix_, "joint_6"),
00319                                                     concatTfName(tf_prefix_, "end_effector")));
00320 }
00321 
00322 }  // namespace jaco

jaco_driver
Author(s): Ilia Baranov (Clearpath) , Jeff Schmidt (Clearpath) , Alex Bencz (Clearpath) , Matt DeDonato (WPI), Braden Stenning
autogenerated on Mon Mar 2 2015 16:21:03