jaco_driver: jaco_arm_kinematics.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * jacolib.cpp
00003  *
00004  *  Created on: Mar 10, 2013
00005  *      Author: mdedonato
00006  */
00007 #include <jaco_driver/jaco_arm_kinematics.h>
00008 
00009 namespace jaco {
00010 JacoKinematics::JacoKinematics(void) {
00011 
00012 }
00013 void JacoKinematics::UpdateForward(float q1, float q2, float q3, float q4,
00014                 float q5, float q6) {
00015         tf::Transform transform;
00016         tf::Quaternion rotation_q(0, 0, 0, 0);
00017         tf::Matrix3x3 rot_matrix(0, 0, 0, 0, 0, 0, 0, 0, 0);
00018         tf::Vector3 translation_v(0, 0, 0);
00019 
00020         /**********************Base**********************/
00021                 /* API Rotation */
00022                 rotation_q.setValue(0, 0, 0, 0); //zero rotation
00023                 rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00024 
00025 
00026                 /* API Rotation Matrix */
00027                 /*******                                                         *******
00028                  * cos(PI/2)            -sin(PI/2)               0 *
00029                  * sin(PI/2)                cos(PI/2)                0 *
00030                  * 0                                    0                                        1 *
00031                  *******                                                             *******/
00032                 rot_matrix.setValue(cos(M_PI_2), -sin(M_PI_2), 0, sin(M_PI_2), cos(M_PI_2), 0, 0, 0, 1);
00033                 rot_matrix.getRotation(rotation_q);
00034 
00035         #ifdef PRINT_DEBUG_INFO
00036                 /* Display Results */
00037                 ROS_INFO(
00038                                 "API Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00039         #endif
00040                 transform.setRotation(rotation_q); //Set Rotation
00041 
00042                 /* API Translation */
00043                 translation_v.setValue(0, 0, 0); //zero translation
00044                 //get API translation vector
00045 
00046                 /* API Translation Vector */
00047                 /****     ****
00048                  * 0             *
00049                  * 0             *
00050                  * 0        *
00051                  ****     ****/
00052                 translation_v.setValue(0, 0, 0);
00053 
00054         #ifdef PRINT_DEBUG_INFO
00055 
00056                 /* Display Results */
00057                 ROS_INFO(
00058                                 "API Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00059         #endif
00060 
00061                 transform.setOrigin(translation_v);     //Set Translation
00062 
00063                 /* Broadcast Transform */
00064                 br.sendTransform(
00065                                 tf::StampedTransform(transform, ros::Time::now(), "arm_base",
00066                                                 "jaco_base"));
00067                 /***************************************************/
00068                 /**********************API**********************/
00069                                 /* API Rotation */
00070                                 rotation_q.setValue(0, 0, 0, 0); //zero rotation
00071                                 rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00072 
00073 
00074                                 /* API Rotation Matrix */
00075                                 /*******                                                                    *******
00076                                  * cos(deg_to_rad(15))          -sin(deg_to_rad(15))            0 *
00077                                  * sin(deg_to_rad(15))          cos(deg_to_rad(15))             0 *
00078                                  * 0                                        0                                           1 *
00079                                  *******                                                                        *******/
00080                                 //rot_matrix.setValue(cos(deg_to_rad(16.5)), -sin(deg_to_rad(16.5)), 0, sin(deg_to_rad(16.5)), cos(deg_to_rad(16.5)), 0, 0, 0, 1);
00081                                 rot_matrix.setValue(1,0,0,0,1,0,0,0,1);
00082 
00083                                 rot_matrix.getRotation(rotation_q);
00084 
00085                         #ifdef PRINT_DEBUG_INFO
00086                                 /* Display Results */
00087                                 ROS_INFO(
00088                                                 "API Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00089                         #endif
00090                                 transform.setRotation(rotation_q); //Set Rotation
00091 
00092                                 /* API Translation */
00093                                 translation_v.setValue(0, 0, 0); //zero translation
00094                                 //get API translation vector
00095 
00096                                 /* API Translation Vector */
00097                                 /****     ****
00098                                  * 0             *
00099                                  * 0             *
00100                                  * 0        *
00101                                  ****     ****/
00102                                 translation_v.setValue(0, 0, 0.028);
00103 
00104                         #ifdef PRINT_DEBUG_INFO
00105 
00106                                 /* Display Results */
00107                                 ROS_INFO(
00108                                                 "API Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00109                         #endif
00110 
00111                                 transform.setOrigin(translation_v);     //Set Translation
00112 
00113                                 /* Broadcast Transform */
00114                                 br.sendTransform(
00115                                                 tf::StampedTransform(transform, ros::Time::now(), "jaco_base",
00116                                                                 "jaco_api_origin"));
00117                                 /***************************************************/
00118 
00119 
00120         /**********************Joint_1**********************/
00121         /* Joint 1 Rotation */
00122         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00123         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00124 
00125         /* Joint 1 Rotation Matrix */
00126         /*******                                         *******
00127          * cos(q1)              -sin(q1)                    0  *
00128          * -sin(q1)             -cos(q1)        0  *
00129          * 0                    0                               -1 *
00130          *******                                     *******/
00131 
00132         rot_matrix.setValue(cos(q1), -sin(q1), 0, -sin(q1), -cos(q1), 0, 0, 0, -1);
00133         rot_matrix.getRotation(rotation_q);
00134 
00135 #ifdef PRINT_DEBUG_INFO
00136         /* Display Results */
00137         ROS_INFO(
00138                         "Joint 1 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00139 #endif
00140         transform.setRotation(rotation_q); //Set Rotation
00141 
00142         /* Joint 1 Translation */
00143         translation_v.setValue(0, 0, 0); //zero translation
00144         //get joint 1 translation vector
00145 
00146         /* Joint 1 Translation Vector */
00147         /****     ****
00148          * 0             *
00149          * 0             *
00150          * D1        *
00151          ****     ****/
00152         translation_v.setValue(0, 0, this->BaseToJ1());
00153 
00154 #ifdef PRINT_DEBUG_INFO
00155 
00156         /* Display Results */
00157         ROS_INFO(
00158                         "Joint 1 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00159 #endif
00160 
00161         transform.setOrigin(translation_v);     //Set Translation
00162 
00163         /* Broadcast Transform */
00164         br.sendTransform(
00165                         tf::StampedTransform(transform, ros::Time::now(), "jaco_base",
00166                                         "jaco_joint_1"));
00167         /***************************************************/
00168 
00169         /**********************Joint_2**********************/
00170         /* Joint 2 Rotation */
00171         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00172         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00173         /* Joint 2 Rotation Matrix */
00174         /*******                                         *******
00175          * sin(q2)              cos(q2)         0  *
00176          * 0                    0                               1 *
00177          * cos(q2)              -sin(q2)        0  *
00178          *******                                     *******/
00179         rot_matrix.setValue(sin(q2), cos(q2), 0, 0, 0, 1, cos(q2), -sin(q2), 0);
00180 
00181         rot_matrix.getRotation(rotation_q);
00182 
00183 #ifdef PRINT_DEBUG_INFO
00184         /* Display Results */
00185         ROS_INFO(
00186                         "Joint 2 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00187 #endif
00188         transform.setRotation(rotation_q); //Set Rotation
00189 
00190         /* Joint 2 Translation */
00191         translation_v.setValue(0, 0, 0); //zero translation
00192         //get joint 2 translation vector
00193 
00194         /* Joint 2 Translation Vector */
00195         /****     ****
00196          * 0             *
00197          * 0             *
00198          * -D1        *
00199          ****     ****/
00200         translation_v.setValue(0, 0 , -this->J1ToJ2());
00201 
00202 #ifdef PRINT_DEBUG_INFO
00203 
00204         /* Display Results */
00205         ROS_INFO(
00206                         "Joint 2 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00207 #endif
00208 
00209         transform.setOrigin(translation_v);     //Set Translation
00210 
00211         /* Broadcast Transform */
00212         br.sendTransform(
00213                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_1",
00214                                         "jaco_joint_2"));
00215         /***************************************************/
00216 
00217         /**********************Joint_3**********************/
00218         /* Joint 3 Rotation */
00219         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00220         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00221         /* Joint 3 Rotation Matrix */
00222         /*******                                        *******
00223          * -cos(q3)             sin(q3)         0 *
00224          * sin(q2)              cos(q3)         0 *
00225          * 0                    0                   -1 *
00226          *******                                    *******/
00227         rot_matrix.setValue(-cos(q3), sin(q3), 0, sin(q3), cos(q3), 0, 0, 0, -1);
00228 
00229         rot_matrix.getRotation(rotation_q);
00230 
00231 #ifdef PRINT_DEBUG_INFO
00232         /* Display Results */
00233         ROS_INFO(
00234                         "Joint 3 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00235 #endif
00236         transform.setRotation(rotation_q); //Set Rotation
00237 
00238         /* Joint 3 Translation */
00239         translation_v.setValue(0, 0, 0); //zero translation
00240         //get joint 3 translation vector
00241 
00242         /* Joint 3 Translation Vector */
00243         /****     ****
00244          * cos(q2)       *
00245          * sin(q2)       *
00246          * D2        *
00247          ****     ****/
00248         translation_v.setValue(this->J2ToJ3(), 0, 0);
00249 
00250 #ifdef PRINT_DEBUG_INFO
00251 
00252         /* Display Results */
00253         ROS_INFO(
00254                         "Joint 3 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00255 #endif
00256 
00257         transform.setOrigin(translation_v);     //Set Translation
00258 
00259         /* Broadcast Transform */
00260         br.sendTransform(
00261                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_2",
00262                                         "jaco_joint_3"));
00263         /***************************************************/
00264 
00265         /**********************Joint_3 Offset**********************/
00266         /* Joint 3 Rotation */
00267         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00268         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00269         /* Joint 3 Rotation Matrix */
00270         /*******                                        *******
00271          * -cos(q3)             sin(q3)         0 *
00272          * sin(q2)              cos(q3)         0 *
00273          * 0                    0                   -1 *
00274          *******                                    *******/
00275         rot_matrix.setValue(1, 0, 0, 0, 1, 0, 0, 0, 1);
00276 
00277         rot_matrix.getRotation(rotation_q);
00278 
00279 #ifdef PRINT_DEBUG_INFO
00280         /* Display Results */
00281         ROS_INFO(
00282                         "Joint 3 Offset Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00283 #endif
00284         transform.setRotation(rotation_q); //Set Rotation
00285 
00286         /* Joint 3 Translation */
00287         translation_v.setValue(0, 0, 0); //zero translation
00288         //get joint 3 translation vector
00289 
00290         /* Joint 3 Translation Vector */
00291         /****     ****
00292          * cos(q2)       *
00293          * sin(q2)       *
00294          * D2        *
00295          ****     ****/
00296         translation_v.setValue(0, 0, -this->J3Offset());
00297 
00298 #ifdef PRINT_DEBUG_INFO
00299 
00300         /* Display Results */
00301         ROS_INFO(
00302                         "Joint 3 Offset Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00303 #endif
00304 
00305         transform.setOrigin(translation_v);     //Set Translation
00306 
00307         /* Broadcast Transform */
00308         br.sendTransform(
00309                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_3",
00310                                         "jaco_joint_3_offset"));
00311         /***************************************************/
00312 
00313 
00314         /**********************Joint_4**********************/
00315         /* Joint 4 Rotation */
00316         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00317         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00318         /* Joint 4 Rotation Matrix */
00319         /*******                                        *******
00320          * 0                    0                          -1 *
00321          * sin(q4)              cos(q4)                 0 *
00322          * cos(q4)              -sin(q4)            0 *
00323          *******                                    *******/
00324         rot_matrix.setValue(0,0,-1, sin(q4), cos(q4), 0, cos(q4), -sin(q4), 0);
00325 
00326         rot_matrix.getRotation(rotation_q);
00327 
00328 #ifdef PRINT_DEBUG_INFO
00329         /* Display Results */
00330         ROS_INFO(
00331                         "Joint 4 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00332 #endif
00333         transform.setRotation(rotation_q); //Set Rotation
00334 
00335         /* Joint 4 Translation */
00336         translation_v.setValue(0, 0, 0); //zero translation
00337         //get joint 4 translation vector
00338 
00339         /* Joint 4 Translation Vector */
00340         /****     ****
00341          * cos(q2)       *
00342          * sin(q2)       *
00343          * D2        *
00344          ****     ****/
00345         translation_v.setValue(this->J3ToJ4(), 0, 0);
00346 
00347 #ifdef PRINT_DEBUG_INFO
00348 
00349         /* Display Results */
00350         ROS_INFO(
00351                         "Joint 4 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00352 #endif
00353 
00354         transform.setOrigin(translation_v);     //Set Translation
00355 
00356         /* Broadcast Transform */
00357         br.sendTransform(
00358                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_3_offset",
00359                                         "jaco_joint_4"));
00360         /***************************************************/
00361 
00362         /**********************Joint_5**********************/
00363         /* Joint 5 Rotation */
00364         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00365         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00366 
00367         /* Joint 5 Rotation Matrix */
00368         /*******                                                                                *******
00369          * cos(-35))*cos(q5)            cos(-35)*-sin(q5)               sin(-35) *
00370          * sin(q5)                                              cos(q5)                                         0             *
00371          * -sin(-35)*cos(q5)            sin(-35)*sin(q5)                cos(-35) *
00372          *******                                                                                                                    *******/
00373         rot_matrix.setValue(cos((this->deg_to_rad(-55)))*cos(q5),cos((this->deg_to_rad(-55)))*-sin(q5),sin((this->deg_to_rad(-55))), sin(q5), cos(q5), 0, -sin((this->deg_to_rad(-55)))*cos(q5), sin((this->deg_to_rad(-55)))*sin(q5), cos((this->deg_to_rad(-55))));
00374 
00375         rot_matrix.getRotation(rotation_q);
00376 
00377 #ifdef PRINT_DEBUG_INFO
00378         /* Display Results */
00379         ROS_INFO(
00380                         "Joint 5 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00381 #endif
00382         transform.setRotation(rotation_q); //Set Rotation
00383 
00384         /* Joint 5 Translation */
00385         translation_v.setValue(0, 0, 0); //zero translation
00386         //get joint 5 translation vector
00387 
00388         /* Joint 5 Translation Vector */
00389         /****     ****
00390          * cos(55)*D4    *
00391          * 0      *
00392          * -sin(55)*D4      *
00393          ****     ****/
00394         translation_v.setValue(cos(this->deg_to_rad(55))*this->J4ToJ5(), 0, -sin(this->deg_to_rad(55))*this->J4ToJ5());
00395 
00396 #ifdef PRINT_DEBUG_INFO
00397 
00398         /* Display Results */
00399         ROS_INFO(
00400                         "Joint 5 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00401 #endif
00402 
00403         transform.setOrigin(translation_v);     //Set Translation
00404 
00405         /* Broadcast Transform */
00406         br.sendTransform(
00407                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_4",
00408                                         "jaco_joint_5"));
00409         /***************************************************/
00410 
00411 
00412         /**********************Joint_6**********************/
00413         /* Joint 6 Rotation */
00414         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00415         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00416 
00417         /* Joint 6 Rotation Matrix */
00418         /*******                                                                                *******
00419          * cos(-35))*cos(q6)            cos(-35)*-sin(q6)               sin(-35) *
00420          * sin(q6)                                              cos(q6)                                         0             *
00421          * -sin(-35)*cos(q6)            sin(-35)*sin(q6)                cos(-35) *
00422          *******                                                                                                                    *******/
00423         rot_matrix.setValue(cos((this->deg_to_rad(55)))*cos(q6),cos((this->deg_to_rad(55)))*-sin(q6),sin((this->deg_to_rad(55))), sin(q6), cos(q6), 0, -sin((this->deg_to_rad(55)))*cos(q6), sin((this->deg_to_rad(55)))*sin(q6), cos((this->deg_to_rad(55))));
00424 
00425         rot_matrix.getRotation(rotation_q);
00426 
00427 #ifdef PRINT_DEBUG_INFO
00428         /* Display Results */
00429         ROS_INFO(
00430                         "Joint 6 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00431 #endif
00432         transform.setRotation(rotation_q); //Set Rotation
00433 
00434         /* Joint 6 Translation */
00435         translation_v.setValue(0, 0, 0); //zero translation
00436         //get joint 6 translation vector
00437 
00438         /* Joint 6 Translation Vector */
00439         /****     ****
00440          * -cos(55)*D5   *
00441          * 0     *
00442          * -sin(55)*D5       *
00443          ****     ****/
00444         translation_v.setValue(-cos(this->deg_to_rad(55))*this->J5ToJ6(), 0, -sin(this->deg_to_rad(55))*this->J5ToJ6());
00445 
00446 #ifdef PRINT_DEBUG_INFO
00447 
00448         /* Display Results */
00449         ROS_INFO(
00450                         "Joint 6 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00451 #endif
00452 
00453         transform.setOrigin(translation_v);     //Set Translation
00454 
00455         /* Broadcast Transform */
00456         br.sendTransform(
00457                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_5",
00458                                         "jaco_joint_6"));
00459         /***************************************************/
00460 
00461         /**********************Joint_6**********************/
00462         /* Joint 6 Rotation */
00463         rotation_q.setValue(0, 0, 0, 0); //zero rotation
00464         rot_matrix.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
00465 
00466         /* Joint 6 Rotation Matrix */
00467         /*******                                                                                *******
00468          * cos(-35))*cos(q6)            cos(-35)*-sin(q6)               sin(-35) *
00469          * sin(q6)                                              cos(q6)                                         0             *
00470          * -sin(-35)*cos(q6)            sin(-35)*sin(q6)                cos(-35) *
00471          *******                                                                                                                    *******/
00472         rot_matrix.setValue(1,0,0,0,1,0,0,0,1);
00473 
00474         rot_matrix.getRotation(rotation_q);
00475 
00476 #ifdef PRINT_DEBUG_INFO
00477         /* Display Results */
00478         ROS_INFO(
00479                         "Joint 6 Rotation: X = %f, Y = %f, Z = %f, W = %f", rotation_q.getX(), rotation_q.getY(), rotation_q.getZ(), rotation_q.getW());
00480 #endif
00481         transform.setRotation(rotation_q); //Set Rotation
00482 
00483         /* Joint 6 Translation */
00484         translation_v.setValue(0, 0, 0); //zero translation
00485         //get joint 6 translation vector
00486 
00487         /* Joint 6 Translation Vector */
00488         /****     ****
00489          * -cos(55)*D5   *
00490          * 0     *
00491          * -sin(55)*D5       *
00492          ****     ****/
00493         translation_v.setValue(0,0,-this->J6ToEnd());
00494 
00495 #ifdef PRINT_DEBUG_INFO
00496 
00497         /* Display Results */
00498         ROS_INFO(
00499                         "Joint 6 Translation: X = %f, Y = %f, Z = %f", translation_v.getX(), translation_v.getY(), translation_v.getZ());
00500 #endif
00501 
00502         transform.setOrigin(translation_v);     //Set Translation
00503 
00504         /* Broadcast Transform */
00505         br.sendTransform(
00506                         tf::StampedTransform(transform, ros::Time::now(), "jaco_joint_6",
00507                                         "jaco_end_effector"));
00508         /***************************************************/
00509 
00510 }
00511 }