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