kni: comp_dq.cpp Source File

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 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2003/14/05: Etienne Lachance
00035    -Fix Robot::dq_torque.
00036    -Added mRobot/mRobot_min_para::dq_torque.
00037 
00038 2004/07/01: Etienne Lachance
00039    -Replace vec_x_prod by CrossProduct.
00040 
00041 2004/07/01: Ethan Tira-Thompson
00042     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00043 
00044 2004/07/02: Etienne Lachance
00045    -Added Doxygen comments.
00046 -------------------------------------------------------------------------------
00047 */
00048 
00054 
00055 static const char rcsid[] = "$Id: comp_dq.cpp,v 1.17 2004/07/06 02:16:36 gourdeau Exp $";
00056 
00057 #include "robot.h"
00058 
00059 #ifdef use_namespace
00060 namespace ROBOOP {
00061   using namespace NEWMAT;
00062 #endif
00063 
00064 
00065 void Robot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
00066                       const ColumnVector & qpp, const ColumnVector & dq,
00067                       ColumnVector & ltorque, ColumnVector & dtorque)
00074 {
00075    int i;
00076    Matrix Rt, temp;
00077    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00078    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00079    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00080    if(dq.Ncols() != 1 || qpp.Nrows() != dof) error("dq has wrong dimension");
00081    ltorque = ColumnVector(dof);
00082    dtorque = ColumnVector(dof);
00083    set_q(q);
00084 
00085    vp[0] = gravity;
00086    ColumnVector z0(3);
00087    z0(1) = 0.0; z0(2) = 0.0;  z0(3) = 1.0;
00088    Matrix Q(3,3);
00089    Q = 0.0;
00090    Q(1,2) = -1.0;
00091    Q(2,1) = 1.0;
00092 
00093    for(i = 1; i <= dof; i++)
00094    {
00095       Rt = links[i].R.t();
00096       p[i] = ColumnVector(3);
00097       p[i](1) = links[i].get_a();
00098       p[i](2) = links[i].get_d() * Rt(2,3);
00099       p[i](3) = links[i].get_d() * Rt(3,3);
00100       if(links[i].get_joint_type() != 0)
00101       {
00102          dp[i] = ColumnVector(3);
00103          dp[i](1) = 0.0;
00104          dp[i](2) = Rt(2,3);
00105          dp[i](3) = Rt(3,3);
00106       }
00107       if(links[i].get_joint_type() == 0)
00108       {
00109          w[i] = Rt*(w[i-1] + z0*qp(i));
00110          dw[i] = Rt*(dw[i-1] - Q*w[i-1]*dq(i));
00111          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00112                      + CrossProduct(w[i-1],z0*qp(i)));
00113          dwp[i] = Rt*(dwp[i-1]
00114                       + CrossProduct(dw[i-1],z0*qp(i))
00115                       - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
00116                       *dq(i));
00117          vp[i] = CrossProduct(wp[i],p[i])
00118                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00119                  + Rt*(vp[i-1]);
00120          dvp[i] = CrossProduct(dwp[i],p[i])
00121                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00122                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00123                   + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
00124       }
00125       else
00126       {
00127          w[i] = Rt*w[i-1];
00128          dw[i] = Rt*dw[i-1];
00129          wp[i] = Rt*wp[i-1];
00130          dwp[i] = Rt*dwp[i-1];
00131          vp[i] = Rt*(vp[i-1] + z0*qpp(i)
00132                      + 2.0*CrossProduct(w[i],Rt*z0*qp(i)))
00133                  + CrossProduct(wp[i],p[i])
00134                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00135          dvp[i] = Rt*(dvp[i-1]
00136                       + 2.0*CrossProduct(dw[i-1],z0*qp(i)))
00137                   + CrossProduct(dwp[i],p[i])
00138                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00139                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00140                   + (CrossProduct(wp[i],dp[i])
00141                      + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
00142                   *dq(i);
00143       }
00144       a[i] = CrossProduct(wp[i],links[i].r)
00145              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00146              + vp[i];
00147       da[i] = CrossProduct(dwp[i],links[i].r)
00148               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00149               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00150               + dvp[i];
00151    }
00152 
00153    for(i = dof; i >= 1; i--)
00154    {
00155       F[i] = a[i] * links[i].m;
00156       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00157       dF[i] = da[i] * links[i].m;
00158       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00159               + CrossProduct(w[i],links[i].I*dw[i]);
00160       if(i == dof)
00161       {
00162          f[i] = F[i];
00163          df[i] = dF[i];
00164          n[i] = CrossProduct(p[i],f[i])
00165                 + CrossProduct(links[i].r,F[i]) + N[i];
00166          dn[i] = CrossProduct(p[i],df[i])
00167                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00168          if(links[i].get_joint_type() != 0)
00169             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00170       }
00171       else
00172       {
00173          f[i] = links[i+1].R*f[i+1] + F[i];
00174          df[i] = links[i+1].R*df[i+1] + dF[i];
00175          if(links[i+1].get_joint_type() == 0)
00176             df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
00177          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00178                 + CrossProduct(links[i].r,F[i]) + N[i];
00179 
00180          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
00181                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00182          if(links[i+1].get_joint_type() == 0)
00183             dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
00184          else
00185             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00186       }
00187       if(links[i].get_joint_type() == 0)
00188       {
00189          temp = ((z0.t()*links[i].R)*n[i]);
00190          ltorque(i) = temp(1,1);
00191          temp = ((z0.t()*links[i].R)*dn[i]);
00192          dtorque(i) = temp(1,1);
00193       }
00194       else
00195       {
00196          temp = ((z0.t()*links[i].R)*f[i]);
00197          ltorque(i) = temp(1,1);
00198          temp = ((z0.t()*links[i].R)*df[i]);
00199          dtorque(i) = temp(1,1);
00200       }
00201    }
00202 }
00203 
00204 
00205 void mRobot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
00206                        const ColumnVector & qpp, const ColumnVector & dq,
00207                        ColumnVector & ltorque, ColumnVector & dtorque)
00214 {
00215    int i;
00216    Matrix Rt, temp;
00217    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00218    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00219    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00220    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00221    ltorque = ColumnVector(dof);
00222    dtorque = ColumnVector(dof);
00223    set_q(q);
00224 
00225    vp[0] = gravity;
00226    ColumnVector z0(3);
00227    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00228    Matrix Q(3,3);
00229    Q = 0.0;
00230    Q(1,2) = -1.0;
00231    Q(2,1) = 1.0;
00232    for(i = 1; i <= dof; i++)
00233    {
00234       Rt = links[i].R.t();
00235       p[i] = links[i].p;
00236       if(links[i].get_joint_type() != 0)
00237       {
00238          dp[i] = ColumnVector(3);
00239          dp[i](1) = 0.0;
00240          dp[i](2) = Rt(2,3);
00241          dp[i](3) = Rt(3,3);
00242       }
00243       if(links[i].get_joint_type() == 0)
00244       {
00245          w[i] = Rt*w[i-1] + z0*qp(i);
00246          dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
00247          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00248                  + z0*qpp(i);
00249          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00250                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
00251          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00252                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00253                      + vp[i-1]);
00254          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00255                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00256                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00257                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00258                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00259       }
00260       else
00261       {
00262          w[i] = Rt*w[i-1];
00263          dw[i] = Rt*dw[i-1];
00264          wp[i] = Rt*wp[i-1];
00265          dwp[i] = Rt*dwp[i-1];
00266          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00267                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00268                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00269          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00270                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00271                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00272                       + (CrossProduct(wp[i-1],dp[i])
00273                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00274                   + 2*CrossProduct(dw[i],z0*qp(i));
00275       }
00276       a[i] = CrossProduct(wp[i],links[i].r)
00277              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00278              + vp[i];
00279       da[i] = CrossProduct(dwp[i],links[i].r)
00280               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00281               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00282               + dvp[i];
00283    }
00284 
00285    for(i = dof; i >= 1; i--) {
00286       F[i] = a[i] * links[i].m;
00287       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00288       dF[i] = da[i] * links[i].m;
00289       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00290               + CrossProduct(w[i],links[i].I*dw[i]);
00291 
00292       if(i == dof)
00293       {
00294          f[i] = F[i];
00295          df[i] = dF[i];
00296          n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00297          dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
00298       }
00299       else
00300       {
00301          f[i] = links[i+1].R*f[i+1] + F[i];
00302          df[i] = links[i+1].R*df[i+1] + dF[i];
00303          if(links[i+1].get_joint_type() == 0)
00304             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00305 
00306          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00307                 + CrossProduct(links[i].r,F[i]) + N[i];
00308          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00309                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00310          if(links[i+1].get_joint_type() == 0)
00311             dn[i] += (links[i+1].R*Q*n[i+1] +
00312                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00313          else
00314             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00315       }
00316 
00317       if(links[i].get_joint_type() == 0)
00318       {
00319          temp = z0.t()*n[i];
00320          ltorque(i) = temp(1,1);
00321          temp = z0.t()*dn[i];
00322          dtorque(i) = temp(1,1);
00323       }
00324       else
00325       {
00326          temp = z0.t()*f[i];
00327          ltorque(i) = temp(1,1);
00328          temp = z0.t()*df[i];
00329          dtorque(i) = temp(1,1);
00330       }
00331    }
00332 }
00333 
00334 
00335 void mRobot_min_para::dq_torque(const ColumnVector & q, const ColumnVector & qp,
00336                                 const ColumnVector & qpp, const ColumnVector & dq,
00337                                 ColumnVector & ltorque, ColumnVector & dtorque)
00344 {
00345    int i;
00346    Matrix Rt, temp;
00347    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00348    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00349    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00350    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00351    ltorque = ColumnVector(dof);
00352    dtorque = ColumnVector(dof);
00353    set_q(q);
00354 
00355    vp[0] = gravity;
00356    ColumnVector z0(3);
00357    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00358    Matrix Q(3,3);
00359    Q = 0.0;
00360    Q(1,2) = -1.0;
00361    Q(2,1) = 1.0;
00362    for(i = 1; i <= dof; i++)
00363    {
00364       Rt = links[i].R.t();
00365       p[i] = links[i].p;
00366       if(links[i].get_joint_type() != 0)
00367       {
00368          dp[i] = ColumnVector(3);
00369          dp[i](1) = 0.0;
00370          dp[i](2) = Rt(2,3);
00371          dp[i](3) = Rt(3,3);
00372       }
00373       if(links[i].get_joint_type() == 0)
00374       {
00375          w[i] = Rt*w[i-1] + z0*qp(i);
00376          dw[i] = Rt*dw[i-1] - Q*Rt*w[i-1]*dq(i);
00377          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00378                  + z0*qpp(i);
00379          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00380                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i);
00381          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00382                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00383                      + vp[i-1]);
00384          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00385                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00386                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00387                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00388                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00389       }
00390       else
00391       {
00392          w[i] = Rt*w[i-1];
00393          dw[i] = Rt*dw[i-1];
00394          wp[i] = Rt*wp[i-1];
00395          dwp[i] = Rt*dwp[i-1];
00396          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00397                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00398                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00399          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00400                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00401                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00402                       + (CrossProduct(wp[i-1],dp[i])
00403                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i));
00404       }
00405    }
00406 
00407    for(i = dof; i >= 1; i--) {
00408       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00409              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00410       dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
00411               + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
00412               + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
00413       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
00414              - CrossProduct(vp[i], links[i].mc);
00415       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00416               + CrossProduct(w[i],links[i].I*dw[i])
00417               - CrossProduct(dvp[i],links[i].mc);
00418 
00419       if(i == dof)
00420       {
00421          f[i] = F[i];
00422          df[i] = dF[i];
00423          n[i] = N[i];
00424          dn[i] = dN[i];
00425       }
00426       else
00427       {
00428          f[i] = links[i+1].R*f[i+1] + F[i];
00429          df[i] = links[i+1].R*df[i+1] + dF[i];
00430          if(links[i+1].get_joint_type() == 0)
00431             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00432 
00433          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00434                 + N[i];
00435          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00436                  + dN[i];
00437          if(links[i+1].get_joint_type() == 0)
00438             dn[i] += (links[i+1].R*Q*n[i+1] +
00439                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00440          else
00441             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00442       }
00443 
00444       if(links[i].get_joint_type() == 0)
00445       {
00446          temp = z0.t()*n[i];
00447          ltorque(i) = temp(1,1);
00448          temp = z0.t()*dn[i];
00449          dtorque(i) = temp(1,1);
00450       }
00451       else
00452       {
00453          temp = z0.t()*f[i];
00454          ltorque(i) = temp(1,1);
00455          temp = z0.t()*df[i];
00456          dtorque(i) = temp(1,1);
00457       }
00458    }
00459 }
00460 
00461 #ifdef use_namespace
00462 }
00463 #endif