dynamics.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    -Member function inertia and acceleration are now part of class Robot_basic.
00035    -Added torque member funtions to allowed to had load on last link.
00036    -Changed variable "n" and "f" for "n_nv" and "f_nv" in torque_novelocity.
00037    -Corrected calculation of wp, vp and n in mRobot::torque and 
00038     mRobot::torque_novelocity.
00039    -Removed all member functions related to classes RobotMotor and mRobotMotor.
00040    -Added motor effect in torque function (see ltorque).
00041    -Added function call set_qp() and set_qpp in Robot::torque and mRobot::torque.
00042 
00043 2003/04/29: Etienne Lachance
00044    -Corrected vp calculation for prismatic case in mRobot/mRobot_min_para::torque, 
00045     mRobot_min_para::torque_novelocity.
00046    -Added functions Robot_basic::acceleration(const ColumnVector & q,const ColumnVector & qp,
00047                                               const ColumnVector & tau)
00048 2003/11/18: Etienne Lachance
00049    -Added member function G() (gravity torque) and C() (Coriolis and centrifugal).
00050 
00051 2004/05/14: Etienne Lachance
00052    -Replaced vec_x_prod by CrossProduct.
00053 
00054 2004/05/21: Etienne Lachance
00055    -Added doxygen comments.
00056 
00057 2004/07/01: Ethan Tira-Thompson
00058     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00059 
00060 2004/07/13: Ethan Tira-Thompson
00061     -Re-added the namespace closing brace at the bottom
00062 
00063 2006/05/19: Richard Gourdeau
00064     -Fixed Gear ratio bug for viscous friction (reported by Carmine Lia)
00065 -------------------------------------------------------------------------------
00066 */
00072 
00073 static const char rcsid[] = "$Id: dynamics.cpp,v 1.34 2006/05/19 18:32:30 gourdeau Exp $";
00074 
00075 #include "robot.h"
00076 
00077 #ifdef use_namespace
00078 namespace ROBOOP {
00079   using namespace NEWMAT;
00080 #endif
00081 
00082 
00083 ReturnMatrix Robot_basic::inertia(const ColumnVector & q)
00085 {
00086    Matrix M(dof,dof);
00087    ColumnVector torque(dof);
00088    set_q(q);
00089    for(int i = 1; i <= dof; i++) {
00090       for(int j = 1; j <= dof; j++) {
00091          torque(j) = (i == j ? 1.0 : 0.0);
00092       }
00093       torque = torque_novelocity(torque);
00094       M.Column(i) = torque;
00095    }
00096    M.Release(); return M;
00097 }
00098 
00099 
00100 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q,
00101                                        const ColumnVector & qp,
00102                                        const ColumnVector & tau_cmd)
00104 {
00105    ColumnVector qpp(dof);
00106    qpp = 0.0;
00107    qpp = inertia(q).i()*(tau_cmd-torque(q,qp,qpp));
00108    qpp.Release(); 
00109    return qpp;
00110 }
00111 
00112 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q, const ColumnVector & qp,
00113                                        const ColumnVector & tau_cmd, const ColumnVector & Fext,
00114                                        const ColumnVector & Next)
00127 {
00128    ColumnVector qpp(dof);
00129    qpp = 0.0;
00130    qpp = inertia(q).i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
00131    qpp.Release(); 
00132    return qpp;
00133 }
00134 
00135 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00136                            const ColumnVector & qpp)
00141 {
00142    ColumnVector Fext(3), Next(3);
00143    Fext = 0;
00144    Next = 0;
00145    return torque(q, qp, qpp, Fext, Next);
00146 }
00147 
00148 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00149                            const ColumnVector & qpp, const ColumnVector & Fext,
00150                            const ColumnVector & Next)
00215 {
00216    int i;
00217    ColumnVector ltorque(dof);
00218    Matrix Rt, temp;
00219    if(q.Nrows() != dof) error("q has wrong dimension");
00220    if(qp.Nrows() != dof) error("qp has wrong dimension");
00221    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00222    set_q(q);
00223    set_qp(qp);
00224 
00225    vp[0] = gravity;
00226    for(i = 1; i <= dof; i++) {
00227       Rt = links[i].R.t();
00228       if(links[i].get_joint_type() == 0) {
00229          w[i] = Rt*(w[i-1] + z0*qp(i));
00230          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00231                      + CrossProduct(w[i-1],z0*qp(i)));
00232          vp[i] = CrossProduct(wp[i],p[i])
00233                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00234                  + Rt*(vp[i-1]);
00235       } else {
00236          w[i] = Rt*w[i-1];
00237          wp[i] = Rt*wp[i-1];
00238          vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00239                  + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00240                  + CrossProduct(wp[i],p[i])
00241                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00242       }
00243       a[i] = CrossProduct(wp[i],links[i].r)
00244              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00245              + vp[i];
00246    }
00247 
00248    for(i = dof; i >= 1; i--) {
00249       F[i] =  a[i] * links[i].m;
00250       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00251       if(i == dof) {
00252          f[i] = F[i] + Fext;
00253          n[i] = CrossProduct(p[i],f[i])
00254                 + CrossProduct(links[i].r,F[i]) + N[i] + Next;
00255       } else {
00256          f[i] = links[i+1].R*f[i+1] + F[i];
00257          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00258                 + CrossProduct(links[i].r,F[i]) + N[i];
00259       }
00260       if(links[i].get_joint_type() == 0)
00261          temp = ((z0.t()*links[i].R)*n[i]);
00262       else
00263          temp = ((z0.t()*links[i].R)*f[i]);
00264       ltorque(i) = temp(1,1)
00265                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00266                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00267    }
00268 
00269    ltorque.Release(); return ltorque;
00270 }
00271 
00272 ReturnMatrix Robot::torque_novelocity(const ColumnVector & qpp)
00277 {
00278    int i;
00279    ColumnVector ltorque(dof);
00280    Matrix Rt, temp;
00281    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00282 
00283    vp[0] = 0.0;
00284    for(i = 1; i <= dof; i++) {
00285       Rt = links[i].R.t();
00286       if(links[i].get_joint_type() == 0) {
00287          wp[i] = Rt*(wp[i-1] + z0*qpp(i));
00288          vp[i] = CrossProduct(wp[i],p[i])
00289                  + Rt*(vp[i-1]);
00290       } else {
00291          wp[i] = Rt*wp[i-1];
00292          vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00293                  + CrossProduct(wp[i],p[i]);
00294       }
00295       a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00296    }
00297 
00298    for(i = dof; i >= 1; i--) {
00299       F[i] = a[i] * links[i].m;
00300       N[i] = links[i].I*wp[i];
00301       if(i == dof) {
00302          f_nv[i] = F[i];
00303          n_nv[i] = CrossProduct(p[i],f_nv[i])
00304                    + CrossProduct(links[i].r,F[i]) + N[i];
00305       } else {
00306          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00307          n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i],f_nv[i])
00308                    + CrossProduct(links[i].r,F[i]) + N[i];
00309       }
00310       if(links[i].get_joint_type() == 0)
00311          temp = ((z0.t()*links[i].R)*n_nv[i]);
00312       else
00313          temp = ((z0.t()*links[i].R)*f_nv[i]);
00314       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00315 
00316    }
00317 
00318    ltorque.Release(); return ltorque;
00319 }
00320 
00321 ReturnMatrix Robot::G()
00323 {
00324    int i;
00325    ColumnVector ltorque(dof);
00326    Matrix Rt, temp;
00327 
00328    vp[0] = gravity;
00329    for(i = 1; i <= dof; i++) {
00330       Rt = links[i].R.t();
00331       if(links[i].get_joint_type() == 0)
00332          vp[i] = Rt*(vp[i-1]);
00333       else
00334          vp[i] = Rt*vp[i-1];
00335 
00336       a[i] = vp[i];
00337    }
00338 
00339    for(i = dof; i >= 1; i--) {
00340       F[i] =  a[i] * links[i].m;
00341       if(i == dof) {
00342          f[i] = F[i];
00343          n[i] = CrossProduct(p[i],f[i])
00344                 + CrossProduct(links[i].r,F[i]);
00345       } else {
00346          f[i] = links[i+1].R*f[i+1] + F[i];
00347          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00348                 + CrossProduct(links[i].r,F[i]);
00349       }
00350       if(links[i].get_joint_type() == 0)
00351          temp = ((z0.t()*links[i].R)*n[i]);
00352       else
00353          temp = ((z0.t()*links[i].R)*f[i]);
00354       ltorque(i) = temp(1,1);
00355    }
00356 
00357    ltorque.Release(); return ltorque;
00358 }
00359 
00360 ReturnMatrix Robot::C(const ColumnVector & qp)
00362 {
00363    int i;
00364    ColumnVector ltorque(dof);
00365    Matrix Rt, temp;
00366    if(qp.Nrows() != dof) error("qp has wrong dimension");
00367 
00368    vp[0]=0;
00369    for(i = 1; i <= dof; i++) {
00370       Rt = links[i].R.t();
00371       if(links[i].get_joint_type() == 0) {
00372          w[i] = Rt*(w[i-1] + z0*qp(i));
00373          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00374          vp[i] = CrossProduct(wp[i],p[i])
00375                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00376                  + Rt*(vp[i-1]);
00377       } else {
00378          w[i] = Rt*w[i-1];
00379          wp[i] = Rt*wp[i-1];
00380          vp[i] = Rt*vp[i-1] + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00381                  + CrossProduct(wp[i],p[i])
00382                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00383       }
00384       a[i] = CrossProduct(wp[i],links[i].r)
00385              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00386              + vp[i];
00387    }
00388 
00389    for(i = dof; i >= 1; i--) {
00390       F[i] =  a[i] * links[i].m;
00391       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00392       if(i == dof) {
00393          f[i] = F[i];
00394          n[i] = CrossProduct(p[i],f[i])
00395                 + CrossProduct(links[i].r,F[i]) + N[i];
00396       } else {
00397          f[i] = links[i+1].R*f[i+1] + F[i];
00398          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00399                 + CrossProduct(links[i].r,F[i]) + N[i];
00400       }
00401       if(links[i].get_joint_type() == 0)
00402          temp = ((z0.t()*links[i].R)*n[i]);
00403       else
00404          temp = ((z0.t()*links[i].R)*f[i]);
00405       ltorque(i) = temp(1,1)
00406                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00407    }
00408 
00409    ltorque.Release(); return ltorque;
00410 }
00411 
00412 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00413                             const ColumnVector & qpp)
00415 {
00416    ColumnVector Fext(3), Next(3);
00417    Fext = 0;
00418    Next = 0;
00419    return torque(q, qp, qpp, Fext, Next);
00420 }
00421 
00422 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00423                             const ColumnVector & qpp, const ColumnVector & Fext_,
00424                             const ColumnVector & Next_)
00487 {
00488    int i;
00489    ColumnVector ltorque(dof);
00490    Matrix Rt, temp;
00491    if(q.Nrows() != dof) error("q has wrong dimension");
00492    if(qp.Nrows() != dof) error("qp has wrong dimension");
00493    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00494    set_q(q);
00495    set_qp(qp);
00496 
00497    vp[0] = gravity;
00498    for(i = 1; i <= dof; i++) {
00499       Rt = links[i].R.t();
00500       if(links[i].get_joint_type() == 0) {
00501          w[i] = Rt*w[i-1] + z0*qp(i);
00502          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00503                  + z0*qpp(i);
00504          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00505                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00506                      + vp[i-1]);
00507       } else {
00508          w[i] = Rt*w[i-1];
00509          wp[i] = Rt*wp[i-1];
00510          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00511                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00512                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00513       }
00514       a[i] = CrossProduct(wp[i],links[i].r)
00515              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00516              + vp[i];
00517    }
00518 
00519    // Load on last link
00520    ColumnVector Fext(3), Next(3);
00521    if(fix) // Last link is fix
00522    {
00523       Fext = links[dof+fix].R*Fext_;
00524       Next = links[dof+fix].R*Next_;
00525    }
00526    else
00527    {
00528       Fext = Fext_;
00529       Next = Next_;
00530    }
00531 
00532    for(i = dof; i >= 1; i--) {
00533       F[i] =  a[i] * links[i].m;
00534       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00535       if(i == dof) {
00536          f[i] = F[i] + Fext;
00537          n[i] = CrossProduct(links[i].r,F[i]) + N[i] + Next;
00538       } else {
00539          f[i] = links[i+1].R*f[i+1] + F[i];
00540          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00541                 + CrossProduct(links[i].r,F[i]) + N[i];
00542       }
00543       if(links[i].get_joint_type() == 0)
00544          temp = (z0.t()*n[i]);
00545       else
00546          temp = (z0.t()*f[i]);
00547       ltorque(i) = temp(1,1)
00548                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00549                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00550    }
00551 
00552    ltorque.Release(); return ltorque;
00553 }
00554 
00555 ReturnMatrix mRobot::torque_novelocity(const ColumnVector & qpp)
00557 {
00558    int i;
00559    ColumnVector ltorque(dof);
00560    Matrix Rt, temp;
00561    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00562 
00563    vp[0] = 0.0;
00564    for(i = 1; i <= dof; i++) {
00565       Rt = links[i].R.t();
00566       if(links[i].get_joint_type() == 0) {
00567          wp[i] = Rt*wp[i-1] + z0*qpp(i);
00568          vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00569       } else {
00570          wp[i] = Rt*wp[i-1];
00571          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00572                  + z0*qpp(i);
00573       }
00574       a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00575    }
00576 
00577    for(i = dof; i >= 1; i--) {
00578       F[i] =  a[i] * links[i].m;
00579       N[i] = links[i].I*wp[i];
00580       if(i == dof) {
00581          f_nv[i] = F[i];
00582          n_nv[i] = CrossProduct(links[i].r,F[i]) + N[i];
00583       } else {
00584          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00585          n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
00586                    + CrossProduct(links[i].r,F[i]) + N[i];
00587       }
00588 
00589       if(links[i].get_joint_type() == 0)
00590          temp = (z0.t()*n_nv[i]);
00591       else
00592          temp = (z0.t()*f_nv[i]);
00593       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00594    }
00595 
00596    ltorque.Release(); return ltorque;
00597 }
00598 
00599 ReturnMatrix mRobot::G()
00601 {
00602    int i;
00603    ColumnVector ltorque(dof);
00604    Matrix Rt, temp;
00605 
00606    vp[0] = gravity;
00607    for(i = 1; i <= dof; i++) {
00608       Rt = links[i].R.t();
00609       if(links[i].get_joint_type() == 0)
00610          vp[i] = Rt*vp[i-1];
00611       else
00612          vp[i] = Rt*vp[i-1];
00613       a[i] = vp[i];
00614    }
00615 
00616    for(i = dof; i >= 1; i--) {
00617       F[i] =  a[i] * links[i].m;
00618       if(i == dof) {
00619          f[i] = F[i];
00620          n[i] = CrossProduct(links[i].r,F[i]);
00621       } else {
00622          f[i] = links[i+1].R*f[i+1] + F[i];
00623          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00624                 + CrossProduct(links[i].r,F[i]);
00625       }
00626       if(links[i].get_joint_type() == 0)
00627          temp = (z0.t()*n[i]);
00628       else
00629          temp = (z0.t()*f[i]);
00630       ltorque(i) = temp(1,1);
00631    }
00632 
00633    ltorque.Release(); return ltorque;
00634 }
00635 
00636 ReturnMatrix mRobot::C(const ColumnVector & qp)
00638 {
00639    int i;
00640    ColumnVector ltorque(dof);
00641    Matrix Rt, temp;
00642    if(qp.Nrows() != dof) error("qp has wrong dimension");
00643 
00644    vp[0] = 0;
00645    for(i = 1; i <= dof; i++) {
00646       Rt = links[i].R.t();
00647       if(links[i].get_joint_type() == 0) {
00648          w[i] = Rt*w[i-1] + z0*qp(i);
00649          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
00650          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00651                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00652                      + vp[i-1]);
00653       } else {
00654          w[i] = Rt*w[i-1];
00655          wp[i] = Rt*wp[i-1];
00656          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00657                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00658                  +  2.0*CrossProduct(w[i],z0*qp(i));
00659       }
00660       a[i] = CrossProduct(wp[i],links[i].r)
00661              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00662              + vp[i];
00663    }
00664 
00665    for(i = dof; i >= 1; i--) {
00666       F[i] =  a[i] * links[i].m;
00667       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00668       if(i == dof) {
00669          f[i] = F[i];
00670          n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00671       } else {
00672          f[i] = links[i+1].R*f[i+1] + F[i];
00673          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00674                 + CrossProduct(links[i].r,F[i]) + N[i];
00675       }
00676       if(links[i].get_joint_type() == 0)
00677          temp = (z0.t()*n[i]);
00678       else
00679          temp = (z0.t()*f[i]);
00680       ltorque(i) = temp(1,1)
00681                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00682    }
00683 
00684    ltorque.Release(); return ltorque;
00685 }
00686 
00687 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00688                                      const ColumnVector & qpp)
00690 {
00691    ColumnVector Fext(3), Next(3);
00692    Fext = 0;
00693    Next = 0;
00694    return torque(q, qp, qpp, Fext, Next);
00695 }
00696 
00697 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00698                                      const ColumnVector & qpp, const ColumnVector & Fext_,
00699                                      const ColumnVector & Next_)
00708 {
00709    int i;
00710    ColumnVector ltorque(dof);
00711    Matrix Rt, temp;
00712    if(q.Nrows() != dof) error("q has wrong dimension");
00713    if(qp.Nrows() != dof) error("qp has wrong dimension");
00714    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00715    set_q(q);
00716    set_qp(qp);
00717 
00718    vp[0] = gravity;
00719    for(i = 1; i <= dof; i++) {
00720       Rt = links[i].R.t();
00721       if(links[i].get_joint_type() == 0)
00722       {
00723          w[i] = Rt*w[i-1] + z0*qp(i);
00724          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)))
00725                  + z0*qpp(i);
00726          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00727                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00728                      + vp[i-1]);
00729       }
00730       else
00731       {
00732          w[i] = Rt*w[i-1];
00733          wp[i] = Rt*wp[i-1];
00734          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00735                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00736                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00737       }
00738    }
00739 
00740    ColumnVector Fext(3), Next(3);
00741    if(fix)
00742    {
00743       Fext = links[dof+fix].R*Fext_;
00744       Next = links[dof+fix].R*Next_;
00745    }
00746    else
00747    {
00748       Fext = Fext_;
00749       Next = Next_;
00750    }
00751 
00752    for(i = dof; i >= 1; i--)
00753    {
00754       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00755              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00756       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00757              CrossProduct(-vp[i], links[i].mc);
00758       if(i == dof)
00759       {
00760          f[i] = F[i] + Fext;
00761          n[i] = N[i] + Next;
00762       }
00763       else
00764       {
00765          f[i] = links[i+1].R*f[i+1] + F[i];
00766          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00767       }
00768 
00769       if(links[i].get_joint_type() == 0)
00770          temp = (z0.t()*n[i]);
00771       else
00772          temp = (z0.t()*f[i]);
00773       ltorque(i) = temp(1,1)
00774                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00775                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00776    }
00777 
00778    ltorque.Release(); return ltorque;
00779 }
00780 
00781 
00782 ReturnMatrix mRobot_min_para::torque_novelocity(const ColumnVector & qpp)
00784 {
00785    int i;
00786    ColumnVector ltorque(dof);
00787    Matrix Rt, temp;
00788    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00789 
00790    vp[0] = 0.0;
00791    for(i = 1; i <= dof; i++)
00792    {
00793       Rt = links[i].R.t();
00794       if(links[i].get_joint_type() == 0)
00795       {
00796          wp[i] = Rt*wp[i-1] + z0*qpp(i);
00797          vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00798       }
00799       else
00800       {
00801          wp[i] = Rt*wp[i-1];
00802          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00803                  + z0*qpp(i);
00804       }
00805    }
00806 
00807    for(i = dof; i >= 1; i--)
00808    {
00809       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc);
00810       N[i] = links[i].I*wp[i] + CrossProduct(-vp[i], links[i].mc);
00811       if(i == dof)
00812       {
00813          f_nv[i] = F[i];
00814          n_nv[i] = N[i];
00815       }
00816       else
00817       {
00818          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00819          n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1]) + N[i];
00820       }
00821 
00822       if(links[i].get_joint_type() == 0)
00823          temp = (z0.t()*n_nv[i]);
00824       else
00825          temp = (z0.t()*f_nv[i]);
00826       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00827    }
00828 
00829    ltorque.Release(); return ltorque;
00830 }
00831 
00832 ReturnMatrix mRobot_min_para::G()
00834 {
00835    int i;
00836    ColumnVector ltorque(dof);
00837    Matrix Rt, temp;
00838 
00839    vp[0] = gravity;
00840    for(i = 1; i <= dof; i++) {
00841       Rt = links[i].R.t();
00842       if(links[i].get_joint_type() == 0)
00843          vp[i] = Rt*vp[i-1];
00844       else
00845          vp[i] = Rt*vp[i-1];
00846    }
00847 
00848    for(i = dof; i >= 1; i--)
00849    {
00850       F[i] = vp[i]*links[i].m;
00851       N[i] = CrossProduct(-vp[i], links[i].mc);
00852       if(i == dof)
00853       {
00854          f[i] = F[i];
00855          n[i] = N[i];
00856       }
00857       else
00858       {
00859          f[i] = links[i+1].R*f[i+1] + F[i];
00860          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00861       }
00862 
00863       if(links[i].get_joint_type() == 0)
00864          temp = (z0.t()*n[i]);
00865       else
00866          temp = (z0.t()*f[i]);
00867       ltorque(i) = temp(1,1);
00868    }
00869 
00870    ltorque.Release(); return ltorque;
00871 }
00872 
00873 ReturnMatrix mRobot_min_para::C(const ColumnVector & qp)
00875 {
00876    int i;
00877    ColumnVector ltorque(dof);
00878    Matrix Rt, temp;
00879    if(qp.Nrows() != dof) error("qp has wrong dimension");
00880    set_qp(qp);
00881 
00882    vp[0] = 0;
00883    for(i = 1; i <= dof; i++) {
00884       Rt = links[i].R.t();
00885       if(links[i].get_joint_type() == 0)
00886       {
00887          w[i] = Rt*w[i-1] + z0*qp(i);
00888          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00889          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00890                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00891                      + vp[i-1]);
00892       }
00893       else
00894       {
00895          w[i] = Rt*w[i-1];
00896          wp[i] = Rt*wp[i-1];
00897          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00898                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00899                  + 2.0*CrossProduct(w[i],z0*qp(i));
00900       }
00901    }
00902 
00903    for(i = dof; i >= 1; i--)
00904    {
00905       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00906              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00907       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00908              CrossProduct(-vp[i], links[i].mc);
00909       if(i == dof)
00910       {
00911          f[i] = F[i];
00912          n[i] = N[i];
00913       }
00914       else
00915       {
00916          f[i] = links[i+1].R*f[i+1] + F[i];
00917          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00918       }
00919 
00920       if(links[i].get_joint_type() == 0)
00921          temp = (z0.t()*n[i]);
00922       else
00923          temp = (z0.t()*f[i]);
00924       ltorque(i) = temp(1,1)
00925                    + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00926    }
00927 
00928    ltorque.Release(); return ltorque;
00929 }
00930 
00931 #ifdef use_namespace
00932 }
00933 #endif
00934 


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