openhrp3: jacobi.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * jacobi.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.10.21
00012  */
00013 
00014 #include <cassert>
00015 #include <cstdlib>
00016 #include "chain.h"
00017 
00018 double Chain::ComJacobian(fMat& J, fVec3& com, const char* charname)
00019 {
00020         double m;
00021         J.resize(3, n_dof);
00022         J.zero();
00023         com.zero();
00024         m = root->com_jacobian(J, com, charname);
00025         com /= m;
00026         J /= m;
00027         return m;
00028 }
00029 
00030 double Joint::com_jacobian(fMat& J, fVec3& com, const char* charname)
00031 {
00032         double b_mass, c_mass;
00033         fVec3 b_com(0,0,0), c_com(0,0,0);
00034 //      cerr << name << ": " << i_chain_dof << endl;
00035         c_mass = child->com_jacobian(J, c_com, charname);
00036         // check if this joint should be included
00037         int is_target = false;
00038         if(!charname)
00039         {
00040                 is_target = true;
00041         }
00042         else
00043         {
00044                 char* my_chname = CharName();
00045                 if(my_chname && !strcmp(my_chname, charname))
00046                 {
00047                         is_target = true;
00048                 }
00049         }
00050         // add myself
00051         if(is_target)
00052         {
00053                 static fVec3 abs_com_pos, my_com;
00054                 abs_com_pos.mul(abs_att, loc_com);
00055                 abs_com_pos += abs_pos;
00056                 c_mass += mass;
00057                 my_com.mul(abs_com_pos, mass);
00058                 c_com += my_com;
00059 
00060                 if(n_dof > 0)
00061                 {
00062                         int i;
00063                         // compute Jacobian
00064                         static fVec3 ms; // vector from child com to joint origin, minus s
00065                         static fMat33 msX; // cross matrix of vector ms
00066                         static fVec3 msXaxis;
00067                         ms = c_com - (c_mass * abs_pos);
00068                         msX.cross(ms);
00069                         static fMat33 msXatt;
00070                         msXatt.mul(msX, abs_att);
00071                         msXatt *= -1.0;
00072                         switch(j_type)
00073                         {
00074                         case JROTATE:
00075                                 msXaxis.mul(msXatt, axis);
00076                                 J(0, i_dof) = msXaxis(0);
00077                                 J(1, i_dof) = msXaxis(1);
00078                                 J(2, i_dof) = msXaxis(2);
00079                                 break;
00080                         case JSLIDE:
00081                           //                            msXaxis.mul(msX, axis);
00082                                 msXaxis.mul(abs_att, axis);
00083                                 msXaxis *= c_mass;
00084                                 J(0, i_dof) = msXaxis(0);
00085                                 J(1, i_dof) = msXaxis(1);
00086                                 J(2, i_dof) = msXaxis(2);
00087                                 break;
00088                         case JSPHERE:
00089                                 for(i=0; i<3; i++)
00090                                 {
00091                                         J(0, i_dof+i) = msXatt(0, i);
00092                                         J(1, i_dof+i) = msXatt(1, i);
00093                                         J(2, i_dof+i) = msXatt(2, i);
00094                                 }
00095                                 break;
00096                         case JFREE:
00097                                 for(i=0; i<3; i++)
00098                                 {
00099                                         J(0, i_dof+i) = c_mass * abs_att(0, i);
00100                                         J(1, i_dof+i) = c_mass * abs_att(1, i);
00101                                         J(2, i_dof+i) = c_mass * abs_att(2, i);
00102                                         J(0, i_dof+3+i) = msXatt(0, i);
00103                                         J(1, i_dof+3+i) = msXatt(1, i);
00104                                         J(2, i_dof+3+i) = msXatt(2, i);
00105                                 }
00106                                 break;
00107                         case JFIXED:
00108                                 break;
00109                         case JUNKNOWN:
00110                                 abort ();
00111                         }
00112                 }
00113         }
00114         b_mass = brother->com_jacobian(J, b_com, charname);
00115         com.add(c_com, b_com);
00116         return c_mass + b_mass;
00117 }
00118 
00119 int Joint::CalcJacobian(fMat& J)
00120 {
00121         J.resize(6, chain->n_dof);
00122         J.zero();
00123         calc_jacobian(J, this);
00124         return 0;
00125 }
00126 
00127 int Joint::calc_jacobian(fMat& J, Joint* target)
00128 {
00129         switch(j_type)
00130         {
00131         case JROTATE:
00132                 if(t_given) calc_jacobian_rotate(J, target);
00133                 break;
00134         case JSLIDE:
00135                 if(t_given) calc_jacobian_slide(J, target);
00136                 break;
00137         case JSPHERE:
00138                 if(t_given) calc_jacobian_sphere(J, target);
00139                 break;
00140         case JFREE:
00141                 if(t_given) calc_jacobian_free(J, target);
00142                 break;
00143         default:
00144                 break;
00145         }
00146         parent->calc_jacobian(J, target);
00147         return 0;
00148 }
00149 
00150 int Joint::calc_jacobian_rotate(fMat& J, Joint* target)
00151 {
00152         static fVec3 axis0, pp, lin;
00153         axis0.mul(abs_att, axis);
00154         pp.sub(target->abs_pos, abs_pos);
00155         lin.cross(axis0, pp);
00156         double* a = J.data() + 6*i_dof;
00157         *a = lin(0);
00158         *(++a) = lin(1);
00159         *(++a) = lin(2);
00160         *(++a) = axis0(0);
00161         *(++a) = axis0(1);
00162         *(++a) = axis0(2);
00163 /*      J(0, i_dof) = lin(0);
00164         J(1, i_dof) = lin(1);
00165         J(2, i_dof) = lin(2);
00166         J(3, i_dof) = axis0(0);
00167         J(4, i_dof) = axis0(1);
00168         J(5, i_dof) = axis0(2);
00169 */      return 0;
00170 }
00171 
00172 int Joint::calc_jacobian_slide(fMat& J, Joint* target)
00173 {
00174         double* a = J.data() + 6*i_dof;
00175         static fVec3 axis0;
00176         axis0.mul(abs_att, axis);
00177         *a = axis0(0);
00178         *(++a) = axis0(1);
00179         *(++a) = axis0(2);
00180         *(++a) = 0.0;
00181         *(++a) = 0.0;
00182         *(++a) = 0.0;
00183 /*      J(0, i_dof) = axis0(0);
00184         J(1, i_dof) = axis0(1);
00185         J(2, i_dof) = axis0(2);
00186         J(3, i_dof) = 0.0;
00187         J(4, i_dof) = 0.0;
00188         J(5, i_dof) = 0.0;
00189 */      return 0;
00190 }
00191 
00192 int Joint::calc_jacobian_sphere(fMat& J, Joint* target)
00193 {
00194         static fVec3 axis, axis0, pp, lin;
00195         double* a = J.data() + 6*i_dof;
00196         axis.zero();
00197         for(int i=0; i<3; a+=6, i++)
00198         {
00199                 axis(i) = 1.0;
00200                 axis0.mul(abs_att, axis);
00201                 pp.sub(target->abs_pos, abs_pos);
00202                 lin.cross(axis0, pp);
00203                 *a = lin(0);
00204                 *(a+1) = lin(1);
00205                 *(a+2) = lin(2);
00206                 *(a+3) = axis0(0);
00207                 *(a+4) = axis0(1);
00208                 *(a+5) = axis0(2);
00209 /*              J(0, i_dof+i) = lin(0);
00210                 J(1, i_dof+i) = lin(1);
00211                 J(2, i_dof+i) = lin(2);
00212                 J(3, i_dof+i) = axis0(0);
00213                 J(4, i_dof+i) = axis0(1);
00214                 J(5, i_dof+i) = axis0(2);
00215 */              axis(i) = 0.0;
00216         }
00217         return 0;
00218 }
00219 
00220 int Joint::calc_jacobian_free(fMat& J, Joint* target)
00221 {
00222         double* a = J.data() + 6*i_dof;
00223         static fVec3 axis, axis0, pp, lin;
00224         int i;
00225         axis.zero();
00226         for(i=0; i<3; a+=6, i++)
00227         {
00228                 axis(i) = 1.0;
00229                 axis0.mul(abs_att, axis);
00230                 pp.sub(target->abs_pos, abs_pos);
00231                 lin.cross(axis0, pp);
00232                 *a = axis0(0);
00233                 *(a+1) = axis0(1);
00234                 *(a+2) = axis0(2);
00235                 *(a+3) = 0.0;
00236                 *(a+4) = 0.0;
00237                 *(a+5) = 0.0;
00238                 *(a+18) = lin(0);
00239                 *(a+19) = lin(1);
00240                 *(a+20) = lin(2);
00241                 *(a+21) = axis0(0);
00242                 *(a+22) = axis0(1);
00243                 *(a+23) = axis0(2);
00244 /*              J(0, i_dof+i) = axis0(0);
00245                 J(1, i_dof+i) = axis0(1);
00246                 J(2, i_dof+i) = axis0(2);
00247                 J(3, i_dof+i) = 0.0;
00248                 J(4, i_dof+i) = 0.0;
00249                 J(5, i_dof+i) = 0.0;
00250                 J(0, i_dof+3+i) = lin(0);
00251                 J(1, i_dof+3+i) = lin(1);
00252                 J(2, i_dof+3+i) = lin(2);
00253                 J(3, i_dof+3+i) = axis0(0);
00254                 J(4, i_dof+3+i) = axis0(1);
00255                 J(5, i_dof+3+i) = axis0(2);
00256 */              axis(i) = 0.0;
00257         }
00258         return 0;
00259 }
00260 
00261 int Joint::CalcJdot(fVec& jdot)
00262 {
00263         static fVec save_acc, zero_acc;
00264         static fVec3 space_acc;
00265         save_acc.resize(chain->n_dof);
00266         zero_acc.resize(chain->n_dof);
00267         jdot.resize(6);
00268         zero_acc.zero();
00269 
00270         // save gravity and joint accelerations
00271         space_acc.set(chain->root->loc_lin_acc);
00272         chain->GetJointAcc(save_acc);
00273         // clear gravity and joint accelerations
00274         chain->root->loc_lin_acc.zero();
00275         chain->SetJointAcc(zero_acc);
00276         // update acceleration
00277         chain->CalcAcceleration();
00278         // jdot obtained
00279         jdot(0) = loc_lin_acc(0);
00280         jdot(1) = loc_lin_acc(1);
00281         jdot(2) = loc_lin_acc(2);
00282         jdot(3) = loc_ang_acc(0);
00283         jdot(4) = loc_ang_acc(1);
00284         jdot(5) = loc_ang_acc(2);
00285         // restore original accelerations
00286         chain->root->loc_lin_acc.set(space_acc);
00287         chain->SetJointAcc(save_acc);
00288         chain->CalcAcceleration();
00289         return 0;
00290 }
00291 
00292 /*
00293  * 2nd-order derivatives
00294  */
00295 // J: array of n_dof fMat's
00296 int Joint::CalcJacobian2(fMat* J)
00297 {
00298         int i;
00299         for(i=0; i<chain->n_dof; i++)
00300         {
00301                 J[i].resize(6, chain->n_dof);
00302                 J[i].zero();
00303         }
00304         calc_jacobian_2(J, this);
00305         return 0;
00306 }
00307 
00308 // first layer
00309 // compute J[*](1...6, i_dof)
00310 int Joint::calc_jacobian_2(fMat* J, Joint* target)
00311 {
00312         if(j_type == JROTATE)
00313         {
00314                 target->calc_jacobian_2_rotate_sub(J, target, this);
00315         }
00316         else if(j_type == JSLIDE)
00317         {
00318                 target->calc_jacobian_2_slide_sub(J, target, this);
00319         }
00320         else if(j_type == JSPHERE)
00321         {
00322                 target->calc_jacobian_2_sphere_sub(J, target, this);
00323         }
00324         else if(j_type == JFREE)
00325         {
00326                 target->calc_jacobian_2_free_sub(J, target, this);
00327         }
00328         parent->calc_jacobian_2(J, target);
00329         return 0;
00330 }
00331 
00332 // main computation
00333 // compute J[i_dof](1...6, j1->i_dof)
00334 int Joint::calc_jacobian_2_rotate_sub(fMat* J, Joint* target, Joint* j1)
00335 {
00336         assert(j1->j_type == JROTATE);
00337         static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
00338         if(j_type == JROTATE)
00339         {
00340                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
00341         }
00342         else if(j_type == JSLIDE)
00343         {
00344                 calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
00345         }
00346         else if(j_type == JSPHERE)
00347         {
00348                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
00349                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
00350                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
00351         }
00352         else if(j_type == JFREE)
00353         {
00354                 calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
00355                 calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
00356                 calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
00357                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof+3);
00358                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+4);
00359                 calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+5);
00360         }
00361         parent->calc_jacobian_2_rotate_sub(J, target, j1);
00362         return 0;
00363 }
00364 
00365 int Joint::calc_jacobian_2_slide_sub(fMat* J, Joint* target, Joint* j1)
00366 {
00367         assert(j1->j_type == JSLIDE);
00368         static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
00369         if(j_type == JROTATE)
00370         {
00371                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
00372         }
00373         else if(j_type == JSLIDE)
00374         {
00375         }
00376         else if(j_type == JSPHERE)
00377         {
00378                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
00379                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
00380                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
00381         }
00382         else if(j_type == JFREE)
00383         {
00384                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof+3);
00385                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+4);
00386                 calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+5);
00387         }
00388         parent->calc_jacobian_2_slide_sub(J, target, j1);
00389         return 0;
00390 }
00391 
00392 int Joint::calc_jacobian_2_sphere_sub(fMat* J, Joint* target, Joint* j1)
00393 {
00394         assert(j1->j_type == JSPHERE);
00395         static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
00396         if(j_type == JROTATE)
00397         {
00398                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, axis, i_dof);
00399                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, axis, i_dof);
00400                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, axis, i_dof);
00401         }
00402         else if(j_type == JSLIDE)
00403         {
00404                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, axis, i_dof);
00405                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, axis, i_dof);
00406                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, axis, i_dof);
00407         }
00408         else if(j_type == JSPHERE)
00409         {
00410                 // my x axis
00411                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, x, i_dof);
00412                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof);
00413                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof);
00414                 // my y axis
00415                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, y, i_dof+1);
00416                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
00417                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
00418                 // my z axis
00419                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, z, i_dof+2);
00420                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
00421                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
00422         }
00423         else if(j_type == JFREE)
00424         {
00425                 // my x trans
00426                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, x, i_dof);
00427                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, x, i_dof);
00428                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, x, i_dof);
00429                 // my y trans
00430                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, y, i_dof+1);
00431                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
00432                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
00433                 // my z trans
00434                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, z, i_dof+2);
00435                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
00436                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
00437                 // my x rot
00438                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, x, i_dof+3);
00439                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof+3);
00440                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof+3);
00441                 // my y rot
00442                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, y, i_dof+4);
00443                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+4);
00444                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+4);
00445                 // my z rot
00446                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, z, i_dof+5);
00447                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+5);
00448                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+5);
00449         }
00450         parent->calc_jacobian_2_sphere_sub(J, target, j1);
00451         return 0;
00452 }
00453 
00454 int Joint::calc_jacobian_2_free_sub(fMat* J, Joint* target, Joint* j1)
00455 {
00456         assert(j1->j_type == JFREE);
00457         static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
00458         if(j_type == JROTATE)
00459         {
00460                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, axis, i_dof);
00461                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, axis, i_dof);
00462                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, axis, i_dof);
00463                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, axis, i_dof);
00464                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, axis, i_dof);
00465                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, axis, i_dof);
00466         }
00467         else if(j_type == JSLIDE)
00468         {
00469                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, axis, i_dof);
00470                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, axis, i_dof);
00471                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, axis, i_dof);
00472         }
00473         else if(j_type == JSPHERE)
00474         {
00475                 // my x rot
00476                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, x, i_dof);
00477                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof);
00478                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof);
00479                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, x, i_dof);
00480                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, x, i_dof);
00481                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, x, i_dof);
00482                 // my y rot
00483                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, y, i_dof+1);
00484                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
00485                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
00486                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, y, i_dof+1);
00487                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, y, i_dof+1);
00488                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, y, i_dof+1);
00489                 // my z rot
00490                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, z, i_dof+2);
00491                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
00492                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
00493                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, z, i_dof+2);
00494                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, z, i_dof+2);
00495                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, z, i_dof+2);
00496         }
00497         else if(j_type == JFREE)
00498         {
00499                 // my x trans
00500                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, x, i_dof);
00501                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, x, i_dof);
00502                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, x, i_dof);
00503                 // my y trans
00504                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, y, i_dof+1);
00505                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, y, i_dof+1);
00506                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, y, i_dof+1);
00507                 // my z trans
00508                 calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, z, i_dof+2);
00509                 calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, z, i_dof+2);
00510                 calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, z, i_dof+2);
00511                 // my x rot
00512                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, x, i_dof+3);
00513                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof+3);
00514                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof+3);
00515                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, x, i_dof+3);
00516                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, x, i_dof+3);
00517                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, x, i_dof+3);
00518                 // my y rot
00519                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, y, i_dof+4);
00520                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+4);
00521                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+4);
00522                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, y, i_dof+4);
00523                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, y, i_dof+4);
00524                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, y, i_dof+4);
00525                 // my z rot
00526                 calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, z, i_dof+5);
00527                 calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+5);
00528                 calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+5);
00529                 calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, z, i_dof+5);
00530                 calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, z, i_dof+5);
00531                 calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, z, i_dof+5);
00532         }
00533         parent->calc_jacobian_2_free_sub(J, target, j1);
00534         return 0;
00535 }
00536 
00537 int Joint::calc_jacobian_2_rotate_rotate(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
00538 {
00539         static fVec3 out1, out2;
00540         // 1st term
00541         static fVec3 jk_axis, my_axis, d_jk_axis, dp, dJ;
00542         jk_axis.mul(jk->abs_att, k_axis);
00543         my_axis.mul(abs_att, loc_axis);
00544         if(this != jk && isAscendant(jk))
00545         {
00546                 d_jk_axis.zero();
00547         }
00548         else
00549         {
00550                 d_jk_axis.cross(my_axis, jk_axis);
00551         }
00552         dp.sub(target->abs_pos, jk->abs_pos);
00553         out1.cross(d_jk_axis, dp);
00554         // 2nd term
00555         static fVec3 Jt, Jk;
00556         dp.sub(target->abs_pos, abs_pos);
00557         Jt.cross(my_axis, dp);
00558         if(this != jk && isAscendant(jk))
00559         {
00560                 Jk.zero();
00561         }
00562         else
00563         {
00564                 dp.sub(jk->abs_pos, abs_pos);
00565                 Jk.cross(my_axis, dp);
00566         }
00567         dJ.sub(Jt, Jk);
00568         out2.cross(jk_axis, dJ);
00569         // set
00570         J[loc_index](0, k_index) = out1(0) + out2(0);
00571         J[loc_index](1, k_index) = out1(1) + out2(1);
00572         J[loc_index](2, k_index) = out1(2) + out2(2);
00573         J[loc_index](3, k_index) = d_jk_axis(0);
00574         J[loc_index](4, k_index) = d_jk_axis(1);
00575         J[loc_index](5, k_index) = d_jk_axis(2);
00576         return 0;
00577 }
00578 
00579 int Joint::calc_jacobian_2_slide_rotate(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
00580 {
00581         static fVec3 jk_axis, my_axis, d_jk_axis;
00582         jk_axis.mul(jk->abs_att, k_axis);
00583         my_axis.mul(abs_att, loc_axis);
00584 //      if(isAscendant(jk))
00585         if(this != jk && isAscendant(jk))
00586         {
00587                 d_jk_axis.zero();
00588         }
00589         else
00590         {
00591                 d_jk_axis.cross(my_axis, jk_axis);
00592         }
00593         J[loc_index](0, k_index) = d_jk_axis(0);
00594         J[loc_index](1, k_index) = d_jk_axis(1);
00595         J[loc_index](2, k_index) = d_jk_axis(2);
00596         return 0;
00597 }
00598 
00599 int Joint::calc_jacobian_2_rotate_slide(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
00600 {
00601         if(isAscendant(jk))
00602         {
00603                 static fVec3 jk_axis, my_axis, out1;
00604                 jk_axis.mul(jk->abs_att, k_axis);
00605                 my_axis.mul(abs_att, loc_axis);
00606                 out1.cross(jk_axis, my_axis);
00607                 J[loc_index](0, k_index) = out1(0);
00608                 J[loc_index](1, k_index) = out1(1);
00609                 J[loc_index](2, k_index) = out1(2);
00610         }
00611         return 0;
00612 }
00613 
00614