kni: kni: delta_t.cpp Source 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/29/04: Etienne Lachance
00035    -Fix Robot::delta_torque.
00036    -Added mRobot/mRobot_min_para::delta_torque.
00037 
00038 2004/05/14: Etienne Lachance
00039    -Replaced 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: delta_t.cpp,v 1.17 2005/07/01 16:11:45 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::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00066                          const ColumnVector & qpp, const ColumnVector & dq,
00067                          const ColumnVector & dqp, const ColumnVector & dqpp,
00068                          ColumnVector & ltorque, ColumnVector & dtorque)
00156 {
00157    int i;
00158    Matrix Rt, temp;
00159    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00160    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00161    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00162    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00163    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00164    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00165    ltorque = ColumnVector(dof);
00166    dtorque = ColumnVector(dof);
00167    set_q(q);
00168 
00169    vp[0] = gravity;
00170    ColumnVector z0(3);
00171    z0(1) = 0.0; z0(2) = 0.0;  z0(3) = 1.0;
00172    Matrix Q(3,3);
00173    Q = 0.0;
00174    Q(1,2) = -1.0;
00175    Q(2,1) = 1.0;
00176    for(i = 1; i <= dof; i++)
00177    {
00178       Rt = links[i].R.t();
00179       p[i] = ColumnVector(3);
00180       p[i](1) = links[i].get_a();
00181       p[i](2) = links[i].get_d() * Rt(2,3);
00182       p[i](3) = links[i].get_d() * Rt(3,3);
00183       if(links[i].get_joint_type() != 0)
00184       {
00185          dp[i] = ColumnVector(3);
00186          dp[i](1) = 0.0;
00187          dp[i](2) = Rt(2,3);
00188          dp[i](3) = Rt(3,3);
00189       }
00190       if(links[i].get_joint_type() == 0)
00191       {
00192          w[i] = Rt*(w[i-1] + z0*qp(i));
00193          dw[i] = Rt*(dw[i-1] + z0*dqp(i)
00194                      - Q*(w[i-1] + z0*qp(i))*dq(i));
00195          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00196                      + CrossProduct(w[i-1],z0*qp(i)));
00197          dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
00198                       + CrossProduct(dw[i-1],z0*qp(i))
00199                       + CrossProduct(w[i-1],z0*dqp(i))
00200                       - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
00201                       *dq(i));
00202          vp[i] = CrossProduct(wp[i],p[i])
00203                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00204                  + Rt*(vp[i-1]);
00205          dvp[i] = CrossProduct(dwp[i],p[i])
00206                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00207                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00208                   + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
00209       }
00210       else
00211       {
00212          w[i] = Rt*w[i-1];
00213          dw[i] = Rt*dw[i-1];
00214          wp[i] = Rt*wp[i-1];
00215          dwp[i] = Rt*dwp[i-1];
00216          vp[i] = Rt*(vp[i-1] + z0*qpp(i)
00217                      + 2.0*CrossProduct(w[i-1],z0*qp(i)))
00218                  + CrossProduct(wp[i],p[i])
00219                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00220          dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
00221                       + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
00222                              + CrossProduct(w[i-1],z0*dqp(i))))
00223                   + CrossProduct(dwp[i],p[i])
00224                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00225                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00226                   + (CrossProduct(wp[i],dp[i])
00227                      + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
00228                   *dq(i);
00229       }
00230       a[i] = CrossProduct(wp[i],links[i].r)
00231              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00232              + vp[i];
00233       da[i] = CrossProduct(dwp[i],links[i].r)
00234               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00235               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00236               + dvp[i];
00237    }
00238 
00239    for(i = dof; i >= 1; i--)
00240    {
00241       F[i] = a[i] * links[i].m;
00242       dF[i] = da[i] * links[i].m;
00243       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00244       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00245               + CrossProduct(w[i],links[i].I*dw[i]);
00246       if(i == dof)
00247       {
00248          f[i] = F[i];
00249          df[i] = dF[i];
00250          n[i] = CrossProduct(p[i],f[i])
00251                 + CrossProduct(links[i].r,F[i]) + N[i];
00252          dn[i] = CrossProduct(p[i],df[i])
00253                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00254          if(links[i].get_joint_type() != 0)
00255             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00256       }
00257       else
00258       {
00259          f[i] = links[i+1].R*f[i+1] + F[i];
00260          df[i] = links[i+1].R*df[i+1] + dF[i];
00261          if(links[i].get_joint_type() == 0)
00262             df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
00263          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00264                 + CrossProduct(links[i].r,F[i]) + N[i];
00265          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
00266                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00267          if(links[i].get_joint_type() == 0)
00268             dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
00269          else
00270             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00271       }
00272 
00273       if(links[i].get_joint_type() == 0)
00274       {
00275          temp = ((z0.t()*links[i].R)*n[i]);
00276          ltorque(i) = temp(1,1);
00277          temp = ((z0.t()*links[i].R)*dn[i]);
00278          dtorque(i) = temp(1,1);
00279       }
00280       else
00281       {
00282          temp = ((z0.t()*links[i].R)*f[i]);
00283          ltorque(i) = temp(1,1);
00284          temp = ((z0.t()*links[i].R)*df[i]);
00285          dtorque(i) = temp(1,1);
00286       }
00287    }
00288 }
00289 
00290 
00291 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00292                           const ColumnVector & qpp, const ColumnVector & dq,
00293                           const ColumnVector & dqp, const ColumnVector & dqpp,
00294                           ColumnVector & ltorque, ColumnVector & dtorque)
00384 {
00385    int i;
00386    Matrix Rt, temp;
00387    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00388    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00389    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00390    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00391    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00392    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00393    ltorque = ColumnVector(dof);
00394    dtorque = ColumnVector(dof);
00395    set_q(q);
00396 
00397    vp[0] = gravity;
00398    ColumnVector z0(3);
00399    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00400    Matrix Q(3,3);
00401    Q = 0.0;
00402    Q(1,2) = -1.0;
00403    Q(2,1) = 1.0;
00404    for(i = 1; i <= dof; i++)
00405    {
00406       Rt = links[i].R.t();
00407       p[i] = links[i].p;
00408       if(links[i].get_joint_type() != 0)
00409       {
00410          dp[i] = ColumnVector(3);
00411          dp[i](1) = 0.0;
00412          dp[i](2) = Rt(2,3);
00413          dp[i](3) = Rt(3,3);
00414       }
00415       if(links[i].get_joint_type() == 0)
00416       {
00417          w[i] = Rt*w[i-1] + z0*qp(i);
00418          dw[i] = Rt*dw[i-1] + z0*dqp(i)
00419                  - Q*Rt*w[i-1]*dq(i);
00420          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00421                  + z0*qpp(i);
00422          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00423                   + CrossProduct(Rt*w[i-1],z0*dqp(i))
00424                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00425                   + z0*dqpp(i);
00426          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00427                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00428                      + vp[i-1]);
00429          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00430                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00431                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00432                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00433                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00434       }
00435       else
00436       {
00437          w[i] = Rt*w[i-1];
00438          dw[i] = Rt*dw[i-1];
00439          wp[i] = Rt*wp[i-1];
00440          dwp[i] = Rt*dwp[i-1];
00441          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00442                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00443                  + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
00444 
00445          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00446                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00447                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00448                       + (CrossProduct(wp[i-1],dp[i])
00449                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00450                   + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00451                   + z0*dqpp(i);
00452       }
00453       a[i] = CrossProduct(wp[i],links[i].r)
00454              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00455              + vp[i];
00456       da[i] = CrossProduct(dwp[i],links[i].r)
00457               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00458               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00459               + dvp[i];
00460    }
00461 
00462    for(i = dof; i >= 1; i--) {
00463       F[i] = a[i] * links[i].m;
00464       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00465       dF[i] = da[i] * links[i].m;
00466       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00467               + CrossProduct(w[i],links[i].I*dw[i]);
00468 
00469       if(i == dof)
00470       {
00471          f[i] = F[i];
00472          df[i] = dF[i];
00473          n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00474          dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
00475       }
00476       else
00477       {
00478          f[i] = links[i+1].R*f[i+1] + F[i];
00479          df[i] = links[i+1].R*df[i+1] + dF[i];
00480          if(links[i+1].get_joint_type() == 0)
00481             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00482 
00483          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00484                 + CrossProduct(links[i].r,F[i]) + N[i];
00485          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00486                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00487          if(links[i+1].get_joint_type() == 0)
00488             dn[i] += (links[i+1].R*Q*n[i+1] +
00489                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00490          else
00491             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00492       }
00493 
00494       if(links[i].get_joint_type() == 0)
00495       {
00496          temp = z0.t()*n[i];
00497          ltorque(i) = temp(1,1);
00498          temp = z0.t()*dn[i];
00499          dtorque(i) = temp(1,1);
00500       }
00501       else
00502       {
00503          temp = z0.t()*f[i];
00504          ltorque(i) = temp(1,1);
00505          temp = z0.t()*df[i];
00506          dtorque(i) = temp(1,1);
00507       }
00508    }
00509 }
00510 
00511 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00512                                    const ColumnVector & qpp, const ColumnVector & dq,
00513                                    const ColumnVector & dqp, const ColumnVector & dqpp,
00514                                    ColumnVector & ltorque, ColumnVector & dtorque)
00520 {
00521    int i;
00522    Matrix Rt, temp;
00523    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00524    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00525    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00526    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00527    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00528    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00529    ltorque = ColumnVector(dof);
00530    dtorque = ColumnVector(dof);
00531    set_q(q);
00532 
00533    vp[0] = gravity;
00534    ColumnVector z0(3);
00535    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00536    Matrix Q(3,3);
00537    Q = 0.0;
00538    Q(1,2) = -1.0;
00539    Q(2,1) = 1.0;
00540    for(i = 1; i <= dof; i++)
00541    {
00542       Rt = links[i].R.t();
00543       p[i] = links[i].p;
00544       if(links[i].get_joint_type() != 0)
00545       {
00546          dp[i] = ColumnVector(3);
00547          dp[i](1) = 0.0;
00548          dp[i](2) = Rt(2,3);
00549          dp[i](3) = Rt(3,3);
00550       }
00551       if(links[i].get_joint_type() == 0)
00552       {
00553          w[i] = Rt*w[i-1] + z0*qp(i);
00554          dw[i] = Rt*dw[i-1] + z0*dqp(i)
00555                  - Q*Rt*w[i-1]*dq(i);
00556          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00557                  + z0*qpp(i);
00558          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00559                   + CrossProduct(Rt*w[i-1],z0*dqp(i))
00560                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00561                   + z0*dqpp(i);
00562          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00563                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00564                      + vp[i-1]);
00565          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00566                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00567                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00568                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00569                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00570       }
00571       else
00572       {
00573          w[i] = Rt*w[i-1];
00574          dw[i] = Rt*dw[i-1];
00575          wp[i] = Rt*wp[i-1];
00576          dwp[i] = Rt*dwp[i-1];
00577          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00578                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00579                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00580          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00581                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00582                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00583                       + (CrossProduct(wp[i-1],dp[i])
00584                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00585                   + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00586                   + z0*dqpp(i);
00587       }
00588    }
00589 
00590    for(i = dof; i >= 1; i--) {
00591       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00592              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00593       dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
00594               + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
00595               + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
00596       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
00597              - CrossProduct(vp[i], links[i].mc);
00598       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00599               + CrossProduct(w[i],links[i].I*dw[i])
00600               - CrossProduct(dvp[i],links[i].mc);
00601 
00602       if(i == dof)
00603       {
00604          f[i] = F[i];
00605          df[i] = dF[i];
00606          n[i] = N[i];
00607          dn[i] = dN[i];
00608       }
00609       else
00610       {
00611          f[i] = links[i+1].R*f[i+1] + F[i];
00612          df[i] = links[i+1].R*df[i+1] + dF[i];
00613          if(links[i+1].get_joint_type() == 0)
00614             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00615 
00616          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00617                 + N[i];
00618          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00619                  + dN[i];
00620          if(links[i+1].get_joint_type() == 0)
00621             dn[i] += (links[i+1].R*Q*n[i+1] +
00622                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00623          else
00624             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00625       }
00626 
00627       if(links[i].get_joint_type() == 0)
00628       {
00629          temp = z0.t()*n[i];
00630          ltorque(i) = temp(1,1);
00631          temp = z0.t()*dn[i];
00632          dtorque(i) = temp(1,1);
00633       }
00634       else
00635       {
00636          temp = z0.t()*f[i];
00637          ltorque(i) = temp(1,1);
00638          temp = z0.t()*df[i];
00639          dtorque(i) = temp(1,1);
00640       }
00641    }
00642 }
00643 
00644 #ifdef use_namespace
00645 }
00646 #endif