kinemat.cpp
Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 -------------------------------------------------------------------------------
00031 Revision_history:
00032 
00033 2003/02/03: Etienne Lachance
00034    -Rewrite mRobot::jacobian() since previous version was incorrect.
00035    -Added functions kine_pd().
00036    -Make sur that joint angles (qout) are in [-pi, pi] in inv_kin functions.
00037 
00038 2003/05/18: Etienne Lachance
00039    -Added functions Robot_basic::jacobian_DLS_inv and 
00040     Robot/mRobot/mRobot_min_para::jacobian_dot
00041 
00042 2003/08/22: Etienne Lachance
00043    -Added parameter converge in inv_kin prototype function. It indicates if the
00044     inverse kinematics solution converge.
00045 
00046 2003/11/26: Etienne Lachance
00047    -Use angle conditions only if it converge in inv_kin.
00048 
00049 2004/01/23: Etienne Lachance
00050    -Added const in non reference argument for all functions.
00051 
00052 2004/03/12: Etienne Lachance
00053    -Added logic to set q in inv_kin. 
00054 
00055 2004/04/24: Etienne Lachance
00056    -Moved inv_kin to invkine.cpp.
00057 
00058 2004/05/14: Etienne Lachance
00059    -Replaced vec_x_prod by CrossProduct.
00060 
00061 2004/05/21: Etienne Lachance
00062    -Added Doxygen comments.
00063 
00064 2004/07/01: Ethan Tira-Thompson
00065     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00066 
00067 2004/07/16: Ethan Tira-Thompson
00068     -Supports Link::immobile flag so jacobians and deltas are 0 for immobile joints
00069     -Jacobians will only contain entries for mobile joints - otherwise NaNs result
00070      in later processing
00071     -Added parameters to jacobian functions to generate for frames other than 
00072      the end effector
00073 -------------------------------------------------------------------------------
00074 */
00075 
00081 
00082 static const char rcsid[] = "$Id: kinemat.cpp,v 1.31 2004/08/16 00:37:53 gourdeau Exp $";
00083 
00084 #include "robot.h"
00085 
00086 #ifdef use_namespace
00087 namespace ROBOOP {
00088   using namespace NEWMAT;
00089 #endif
00090 
00091 
00092 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos)const
00098 {
00099    kine(Rot,pos,dof+fix);
00100 }
00101 
00102 void Robot_basic::kine(Matrix & Rot, ColumnVector & pos, const int j)const
00109 {
00110    if(j < 1 || j > dof+fix) error("j must be 1 <= j <= dof+fix");
00111 
00112    Rot = links[1].R;
00113    pos = links[1].p;
00114    for (int i = 2; i <= j; i++) {
00115       pos = pos + Rot*links[i].p;
00116       Rot = Rot*links[i].R;
00117    }
00118 }
00119 
00120 ReturnMatrix Robot_basic::kine(void)const
00122 {
00123    Matrix thomo;
00124 
00125    thomo = kine(dof+fix);
00126    thomo.Release(); return thomo;
00127 }
00128 
00129 ReturnMatrix Robot_basic::kine(const int j)const
00131 {
00132    Matrix Rot, thomo(4,4);
00133    ColumnVector pos;
00134 
00135    kine(Rot,pos,j);
00136    thomo << fourbyfourident;
00137    thomo.SubMatrix(1,3,1,3) = Rot;
00138    thomo.SubMatrix(1,3,4,4) = pos;
00139    thomo.Release(); return thomo;
00140 }
00141 
00142 ReturnMatrix Robot_basic::kine_pd(const int j)const
00153 {
00154    Matrix temp(3,5), Rot;
00155    ColumnVector pos, pos_dot;
00156 
00157    if(j < 1 || j > dof)
00158      error("j must be 1 <= j <= dof");
00159 
00160    kine_pd(Rot, pos, pos_dot, j);
00161 
00162    temp.SubMatrix(1,3,1,3) = Rot;
00163    temp.SubMatrix(1,3,4,4) = pos;
00164    temp.SubMatrix(1,3,5,5) = pos_dot;
00165    temp.Release();
00166    return temp;
00167 }
00168 
00169 ReturnMatrix Robot_basic::jacobian_DLS_inv(const double eps, const double lambda_max,
00170       const int ref)const
00200 {
00201    Matrix jacob_inv_DLS, U, V;
00202    DiagonalMatrix Q;
00203    SVD(jacobian(ref), Q, U, V);
00204 
00205    if(Q(6,6) >= eps)
00206       jacob_inv_DLS = V*Q.i()*U.t();
00207    else
00208    {
00209       Q(6,6) += (1 - pow(Q(6,6)/eps,2))*lambda_max*lambda_max;
00210       jacob_inv_DLS = V*Q.i()*U.t();
00211    }
00212 
00213    jacob_inv_DLS.Release();
00214    return(jacob_inv_DLS);
00215 }
00216 
00217 // ---------------------  R O B O T   DH   N O T A T I O N  --------------------------
00218 
00219 void Robot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00220                     const int j)const
00229 {
00230    if(j < 1 || j > dof)
00231       error("j must be 1 <= j <= dof");
00232 
00233    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00234        pos = ColumnVector(3); 
00235    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00236      pos_dot = ColumnVector(3);
00237 
00238    pos = 0.0;
00239    pos_dot = 0.0;
00240    for(int i = 1; i <= j; i++)
00241    {
00242       R[i] = R[i-1]*links[i].R;
00243       pos = pos + R[i-1]*links[i].p;
00244       pos_dot = pos_dot + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00245    }
00246    Rot = R[j];
00247 }
00248 
00249 void Robot::dTdqi(Matrix & dRot, ColumnVector & dp,
00250                   const int i)
00293 {
00294    int j;
00295    if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00296    if(links[i].get_immobile()) {
00297       dRot = Matrix(3,3);
00298       dp = Matrix(3,1);
00299       dRot = 0.0;
00300       dp = 0.0;
00301    } else if(links[i].get_joint_type() == 0) {
00302       Matrix dR(3,3);
00303       dR = 0.0;
00304       Matrix R2 = links[i].R;
00305       ColumnVector p2 = links[i].p;
00306       dRot = Matrix(3,3);
00307       dRot << threebythreeident;
00308       for(j = 1; j < i; j++) {
00309          dRot = dRot*links[j].R;
00310       }
00311       // dRot * Q
00312       for(j = 1; j <= 3; j++) {
00313          dR(j,1) = dRot(j,2);
00314          dR(j,2) = -dRot(j,1);
00315       }
00316       for(j = i+1; j <= dof; j++) {
00317          p2 = p2 + R2*links[j].p;
00318          R2 = R2*links[j].R;
00319       }
00320       dp = dR*p2;
00321       dRot = dR*R2;
00322    } else {
00323       dRot = Matrix(3,3);
00324       dp = Matrix(3,1);
00325       dRot = 0.0;
00326       dp = 0.0;
00327       dp(3) = 1.0;
00328       for(j = i-1; j >= 1; j--) {
00329          dp = links[j].R*dp;
00330       }
00331    }
00332 }
00333 
00334 ReturnMatrix Robot::dTdqi(const int i)
00341 {
00342    Matrix dRot, thomo(4,4);
00343    ColumnVector dpos;
00344 
00345    dTdqi(dRot, dpos, i);
00346    thomo = (Real) 0.0;
00347    thomo.SubMatrix(1,3,1,3) = dRot;
00348    thomo.SubMatrix(1,3,4,4) = dpos;
00349    thomo.Release(); return thomo;
00350 }
00351 
00352 ReturnMatrix Robot::jacobian(const int endlink, const int ref)const
00401 {
00402    int i, j;
00403    const int adof=get_available_dof(endlink);
00404    Matrix jac(6,adof);
00405    Matrix pr, temp(3,1);
00406 
00407    if(ref < 0 || ref > dof) error("invalid referential");
00408 
00409    for(i = 1; i <= dof; i++) {
00410       R[i] = R[i-1]*links[i].R;
00411       p[i] = p[i-1]+R[i-1]*links[i].p;
00412    }
00413 
00414    for(i=1,j=1; j <= adof; i++) {
00415       if(links[i].get_immobile())
00416          continue;
00417       if(links[i].get_joint_type() == 0) {
00418          temp(1,1) = R[i-1](1,3);
00419          temp(2,1) = R[i-1](2,3);
00420          temp(3,1) = R[i-1](3,3);
00421          pr = p[dof]-p[i-1];
00422          temp = CrossProduct(temp,pr);
00423          jac(1,j) = temp(1,1);
00424          jac(2,j) = temp(2,1);
00425          jac(3,j) = temp(3,1);
00426          jac(4,j) = R[i-1](1,3);
00427          jac(5,j) = R[i-1](2,3);
00428          jac(6,j) = R[i-1](3,3);
00429       } else {
00430          jac(1,j) = R[i-1](1,3);
00431          jac(2,j) = R[i-1](2,3);
00432          jac(3,j) = R[i-1](3,3);
00433          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00434       }
00435       j++;
00436    }
00437 
00438    if(ref != 0) {
00439       Matrix zeros(3,3);
00440       zeros = (Real) 0.0;
00441       Matrix RT = R[ref].t();
00442       Matrix Rot;
00443       Rot = ((RT & zeros) | (zeros & RT));
00444       jac = Rot*jac;
00445    }
00446    jac.Release(); return jac;
00447 }
00448 
00449 ReturnMatrix Robot::jacobian_dot(const int ref)const
00497 {
00498    int i, j;
00499    const int adof=get_available_dof();
00500    Matrix jacdot(6,adof);
00501    ColumnVector e(3), temp, pr, ppr;
00502 
00503    if(ref < 0 || ref > dof)
00504       error("invalid referential");
00505 
00506    for(i = 1; i <= dof; i++)
00507    {
00508       R[i] = R[i-1]*links[i].R;
00509       p[i] = p[i-1] + R[i-1]*links[i].p;
00510       pp[i] = pp[i-1] + CrossProduct(R[i]*w[i], R[i-1]*links[i].p);
00511    }
00512 
00513    for(i=1,j=1; j <= adof; i++) {
00514       if(links[i].get_immobile())
00515                                  continue;
00516       if(links[i].get_joint_type() == 0)
00517       {
00518          pr = p[dof]-p[i-1];
00519          ppr = pp[dof]-pp[i-1];
00520          e(1) = R[i-1](1,3);
00521          e(2) = R[i-1](2,3);
00522          e(3) = R[i-1](3,3);
00523          temp = CrossProduct(R[i-1]*w[i-1], e);
00524          jacdot(4,j) = temp(1);           // d(e)/dt
00525          jacdot(5,j) = temp(2);
00526          jacdot(6,j) = temp(3);
00527          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00528          jacdot(1,j) = temp(1);
00529          jacdot(2,j) = temp(2);
00530          jacdot(3,j) = temp(3);
00531       }
00532       else
00533          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00534             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00535       j++;
00536    }
00537 
00538    if(ref != 0) {
00539       Matrix zeros(3,3);
00540       zeros = (Real) 0.0;
00541       Matrix RT = R[ref].t();
00542       Matrix Rot;
00543       Rot = ((RT & zeros) | (zeros & RT));
00544       jacdot = Rot*jacdot;
00545    }
00546 
00547    jacdot.Release(); return jacdot;
00548 }
00549 
00550 // ----------------  R O B O T   M O D I F I E D   DH   N O T A T I O N  --------------------------
00551 
00552 void mRobot::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00553                      const int j)const
00562 {
00563    if(j < 1 || j > dof+fix)
00564       error("j must be 1 <= j <= dof+fix");
00565 
00566    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00567        pos = ColumnVector(3); 
00568    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00569      pos_dot = ColumnVector(3);
00570 
00571    pos = 0.0;
00572    pos_dot = 0.0;
00573    for(int i = 1; i <= j; i++)
00574    {
00575       pos = pos + R[i-1]*links[i].p;
00576       pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00577       R[i] = R[i-1]*links[i].R;
00578    }
00579    Rot = R[j];
00580 }
00581 
00582 void mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
00618 {
00619    int j;
00620    if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00621    if(links[i].get_immobile()) {
00622       dRot = Matrix(3,3);
00623       dp = Matrix(3,1);
00624       dRot = 0.0;
00625       dp = 0.0;
00626    } else if(links[i].get_joint_type() == 0) {
00627       Matrix dR(3,3), R2(3,3), p2(3,1);
00628       dR = 0.0;
00629       dRot = Matrix(3,3);
00630       dRot << threebythreeident;
00631       for(j = 1; j <= i; j++) {
00632          dRot = dRot*links[j].R;
00633       }
00634       // dRot * Q
00635       for(j = 1; j <= 3; j++) {
00636          dR(j,1) = dRot(j,2);
00637          dR(j,2) = -dRot(j,1);
00638       }
00639       if(i < dof) {
00640          R2 = links[i+1].R;
00641          p2 = links[i+1].p;
00642       } else {
00643          R2 <<  threebythreeident;
00644          p2 = 0.0;
00645       }
00646       for(j = i+1; j <= dof; j++) {
00647          p2 = p2 + R2*links[j].p;
00648          R2 = R2*links[j].R;
00649       }
00650       dp = dR*p2;
00651       dRot = dR*R2;  // probleme ...
00652    } else {
00653       dRot = Matrix(3,3);
00654       dp = Matrix(3,1);
00655       dRot = 0.0;
00656       dp = 0.0;
00657       dp(3) = 1.0;
00658       for(j = i; j >= 1; j--) {
00659          dp = links[j].R*dp;
00660       }
00661    }
00662 }
00663 
00664 ReturnMatrix mRobot::dTdqi(const int i)
00671 {
00672    Matrix dRot, thomo(4,4);
00673    ColumnVector dpos;
00674 
00675    dTdqi(dRot, dpos, i);
00676    thomo = (Real) 0.0;
00677    thomo.SubMatrix(1,3,1,3) = dRot;
00678    thomo.SubMatrix(1,3,4,4) = dpos;
00679    thomo.Release(); return thomo;
00680 }
00681 
00682 ReturnMatrix mRobot::jacobian(const int endlink, const int ref)const
00688 {
00689    int i, j;
00690    const int adof=get_available_dof(endlink);
00691    Matrix jac(6,adof);
00692    ColumnVector pr(3), temp(3);
00693 
00694    if(ref < 0 || ref > dof+fix)
00695       error("invalid referential");
00696 
00697    for(i = 1; i <= dof+fix; i++) {
00698       R[i] = R[i-1]*links[i].R;
00699       p[i] = p[i-1] + R[i-1]*links[i].p;
00700    }
00701 
00702    for(i=1,j=1; j <= adof; i++) {
00703       if(links[i].get_immobile())
00704          continue;
00705       if(links[i].get_joint_type() == 0){
00706          temp(1) = R[i](1,3);
00707          temp(2) = R[i](2,3);
00708          temp(3) = R[i](3,3);
00709          pr = p[dof+fix]-p[i];
00710          temp = CrossProduct(temp,pr);
00711          jac(1,j) = temp(1);
00712          jac(2,j) = temp(2);
00713          jac(3,j) = temp(3);
00714          jac(4,j) = R[i](1,3);
00715          jac(5,j) = R[i](2,3);
00716          jac(6,j) = R[i](3,3);
00717       } else {
00718          jac(1,j) = R[i](1,3);
00719          jac(2,j) = R[i](2,3);
00720          jac(3,j) = R[i](3,3);
00721          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00722       }
00723       j++;
00724    }
00725    if(ref != 0) {
00726       Matrix zeros(3,3);
00727       zeros = (Real) 0.0;
00728       Matrix RT = R[ref].t();
00729       Matrix Rot;
00730       Rot = ((RT & zeros) | (zeros & RT));
00731       jac = Rot*jac;
00732    }
00733    jac.Release(); return jac;
00734 }
00735 
00736 ReturnMatrix mRobot::jacobian_dot(const int ref)const
00742 {
00743    int i, j;
00744    const int adof=get_available_dof();
00745    Matrix jacdot(6,adof);
00746    ColumnVector e(3), temp, pr, ppr;
00747 
00748    if(ref < 0 || ref > dof+fix)
00749       error("invalid referential");
00750 
00751    for(i = 1; i <= dof+fix; i++)
00752    {
00753       R[i] = R[i-1]*links[i].R;
00754       p[i] = p[i-1] + R[i-1]*links[i].p;
00755       pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00756    }
00757 
00758    for(i=1,j=1; j <= adof; i++) {
00759       if(links[i].get_immobile())
00760          continue;
00761       if(links[i].get_joint_type() == 0)
00762       {
00763          pr = p[dof+fix]-p[i];
00764          ppr = pp[dof+fix]-pp[i];
00765 
00766          e(1) = R[i](1,3);
00767          e(2) = R[i](2,3);
00768          e(3) = R[i](3,3);
00769          temp = CrossProduct(R[i-1]*w[i-1], e);
00770          jacdot(4,j) = temp(1);           // d(e)/dt
00771          jacdot(5,j) = temp(2);
00772          jacdot(6,j) = temp(3);
00773 
00774          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
00775          jacdot(1,j) = temp(1);
00776          jacdot(2,j) = temp(2);
00777          jacdot(3,j) = temp(3);
00778       }
00779       else
00780          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
00781             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
00782       j++;
00783    }
00784 
00785    if(ref != 0) {
00786       Matrix zeros(3,3);
00787       zeros = (Real) 0.0;
00788       Matrix RT = R[ref].t();
00789       Matrix Rot;
00790       Rot = ((RT & zeros) | (zeros & RT));
00791       jacdot = Rot*jacdot;
00792    }
00793 
00794    jacdot.Release(); return jacdot;
00795 }
00796 
00797 // ------------- R O B O T  DH  M O D I F I E D,  M I N I M U M   P A R A M E T E R S  ------------
00798 
00799 void mRobot_min_para::kine_pd(Matrix & Rot, ColumnVector & pos, ColumnVector & pos_dot,
00800                               const int j)const
00809 {
00810    if(j < 1 || j > dof+fix)
00811       error("j must be 1 <= j <= dof+fix");
00812 
00813    if( (pos.Nrows()!=3) || (pos.Ncols()!=1) )
00814        pos = ColumnVector(3); 
00815    if( (pos_dot.Nrows()!=3) || (pos_dot.Ncols()!=1) )
00816      pos_dot = ColumnVector(3);
00817 
00818    pos = 0.0;
00819    pos_dot = 0.0;
00820    for(int i = 1; i <= j; i++)
00821    {
00822       pos = pos + R[i-1]*links[i].p;
00823       pos_dot = pos_dot + R[i-1]*CrossProduct(w[i-1], links[i].p);
00824       R[i] = R[i-1]*links[i].R;
00825    }
00826    Rot = R[j];
00827 }
00828 
00829 void mRobot_min_para::dTdqi(Matrix & dRot, ColumnVector & dp, const int i)
00841 {
00842    int j;
00843    if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00844    if(links[i].get_immobile()) {
00845       dRot = Matrix(3,3);
00846       dp = Matrix(3,1);
00847       dRot = 0.0;
00848       dp = 0.0;
00849    } else if(links[i].get_joint_type() == 0) {
00850       Matrix dR(3,3), R2, p2(3,1);
00851       dR = 0.0;
00852       dRot = Matrix(3,3);
00853       dRot << threebythreeident;
00854       for(j = 1; j <= i; j++) {
00855          dRot = dRot*links[j].R;
00856       }
00857       // dRot * Q
00858       for(j = 1; j <= 3; j++) {
00859          dR(j,1) = dRot(j,2);
00860          dR(j,2) = -dRot(j,1);
00861       }
00862       if(i < dof) {
00863          R2 = links[i+1].R;
00864          p2 = links[i+1].p;
00865       } else {
00866          R2 <<  threebythreeident;
00867          p2 = 0.0;
00868       }
00869       for(j = i+1; j <= dof; j++) {
00870          p2 = p2 + R2*links[j].p;
00871          R2 = R2*links[j].R;
00872       }
00873       dp = dR*p2;
00874       dRot = dR*R2;
00875    } else {
00876       dRot = Matrix(3,3);
00877       dp = Matrix(3,1);
00878       dRot = 0.0;
00879       dp = 0.0;
00880       dp(3) = 1.0;
00881       for(j = i; j >= 1; j--) {
00882          dp = links[j].R*dp;
00883       }
00884    }
00885 }
00886 
00887 ReturnMatrix mRobot_min_para::dTdqi(const int i)
00894 {
00895    Matrix dRot, thomo(4,4);
00896    ColumnVector dpos;
00897 
00898    dTdqi(dRot, dpos, i);
00899    thomo = (Real) 0.0;
00900    thomo.SubMatrix(1,3,1,3) = dRot;
00901    thomo.SubMatrix(1,3,4,4) = dpos;
00902    thomo.Release(); return thomo;
00903 }
00904 
00905 ReturnMatrix mRobot_min_para::jacobian(const int endlink, const int ref)const
00911 {
00912    int i, j;
00913    const int adof=get_available_dof(endlink);
00914    Matrix jac(6,adof);
00915    ColumnVector pr(3), temp(3);
00916 
00917    if(ref < 0 || ref > dof+fix)
00918       error("invalid referential");
00919 
00920    for(i = 1; i <= dof+fix; i++) {
00921       R[i] = R[i-1]*links[i].R;
00922       p[i] = p[i-1] + R[i-1]*links[i].p;
00923    }
00924 
00925    for(i=1,j=1; j<=adof; i++) {
00926       if(links[i].get_immobile())
00927          continue;
00928       if(links[i].get_joint_type() == 0){
00929          temp(1) = R[i](1,3);
00930          temp(2) = R[i](2,3);
00931          temp(3) = R[i](3,3);
00932 
00933          pr = p[dof+fix]-p[i];
00934          temp = CrossProduct(temp,pr);
00935          jac(1,j) = temp(1);
00936          jac(2,j) = temp(2);
00937          jac(3,j) = temp(3);
00938          jac(4,j) = R[i](1,3);
00939          jac(5,j) = R[i](2,3);
00940          jac(6,j) = R[i](3,3);
00941       } else {
00942          jac(1,j) = R[i](1,3);
00943          jac(2,j) = R[i](2,3);
00944          jac(3,j) = R[i](3,3);
00945          jac(4,j) = jac(5,j) = jac(6,j) = 0.0;
00946       }
00947       j++;
00948    }
00949    if(ref != 0) {
00950       Matrix zeros(3,3);
00951       zeros = (Real) 0.0;
00952       Matrix RT = R[ref].t();
00953       Matrix Rot;
00954       Rot = ((RT & zeros) | (zeros & RT));
00955       jac = Rot*jac;
00956    }
00957 
00958    jac.Release(); return jac;
00959 }
00960 
00961 ReturnMatrix mRobot_min_para::jacobian_dot(const int ref)const
00967 {
00968    int i, j;
00969    const int adof=get_available_dof();
00970    Matrix jacdot(6,adof);
00971    ColumnVector e(3), temp, pr, ppr;
00972 
00973    if(ref < 0 || ref > dof+fix)
00974       error("invalid referential");
00975 
00976    for(i = 1; i <= dof+fix; i++)
00977    {
00978       R[i] = R[i-1]*links[i].R;
00979       p[i] = p[i-1] + R[i-1]*links[i].p;
00980       pp[i] = pp[i-1] + R[i-1]*CrossProduct(w[i-1], links[i].p);
00981    }
00982 
00983    for(i=1,j=1; j <= dof; i++) {
00984       if(links[i].get_immobile())
00985          continue;
00986       if(links[i].get_joint_type() == 0)
00987       {
00988          pr = p[dof+fix]-p[i];
00989          ppr = pp[dof+fix]-pp[i];
00990 
00991          e(1) = R[i](1,3);
00992          e(2) = R[i](2,3);
00993          e(3) = R[i](3,3);
00994          temp = CrossProduct(R[i-1]*w[i-1], e);
00995          jacdot(4,j) = temp(1);           // d(e)/dt
00996          jacdot(5,j) = temp(2);
00997          jacdot(6,j) = temp(3);
00998 
00999          temp = CrossProduct(temp,pr) + CrossProduct(e,ppr);
01000          jacdot(1,j) = temp(1);
01001          jacdot(2,j) = temp(2);
01002          jacdot(3,j) = temp(3);
01003       }
01004       else
01005          jacdot(1,j) = jacdot(2,j) = jacdot(3,j) =
01006             jacdot(4,j) = jacdot(5,j) = jacdot(6,j) = 0.0;
01007       j++;
01008    }
01009 
01010    if(ref != 0) {
01011       Matrix zeros(3,3);
01012       zeros = (Real) 0.0;
01013       Matrix RT = R[ref].t();
01014       Matrix Rot;
01015       Rot = ((RT & zeros) | (zeros & RT));
01016       jacdot = Rot*jacdot;
01017    }
01018 
01019    jacdot.Release(); return jacdot;
01020 }
01021 
01022 #ifdef use_namespace
01023 }
01024 #endif


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:12