dynamics.cpp

Go to the documentation of this file.

/*
ROBOOP -- A robotics object oriented package in C++
Copyright (C) 1996-2004  Richard Gourdeau

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


Report problems and direct all questions to:

Richard Gourdeau
Professeur Agrege
Departement de genie electrique
Ecole Polytechnique de Montreal
C.P. 6079, Succ. Centre-Ville
Montreal, Quebec, H3C 3A7

email: richard.gourdeau@polymtl.ca
-------------------------------------------------------------------------------
Revision_history:

2003/02/03: Etienne Lachance
   -Member function inertia and acceleration are now part of class Robot_basic.
   -Added torque member funtions to allowed to had load on last link.
   -Changed variable "n" and "f" for "n_nv" and "f_nv" in torque_novelocity.
   -Corrected calculation of wp, vp and n in mRobot::torque and 
    mRobot::torque_novelocity.
   -Removed all member functions related to classes RobotMotor and mRobotMotor.
   -Added motor effect in torque function (see ltorque).
   -Added function call set_qp() and set_qpp in Robot::torque and mRobot::torque.

2003/04/29: Etienne Lachance
   -Corrected vp calculation for prismatic case in mRobot/mRobot_min_para::torque, 
    mRobot_min_para::torque_novelocity.
   -Added functions Robot_basic::acceleration(const ColumnVector & q,const ColumnVector & qp,
                                              const ColumnVector & tau)
2003/11/18: Etienne Lachance
   -Added member function G() (gravity torque) and C() (Coriolis and centrifugal).

2004/05/14: Etienne Lachance
   -Replaced vec_x_prod by CrossProduct.

2004/05/21: Etienne Lachance
   -Added doxygen comments.

2004/07/01: Ethan Tira-Thompson
    -Added support for newmat's use_namespace #define, using ROBOOP namespace

2004/07/13: Ethan Tira-Thompson
    -Re-added the namespace closing brace at the bottom

2006/05/19: Richard Gourdeau
    -Fixed Gear ratio bug for viscous friction (reported by Carmine Lia)
-------------------------------------------------------------------------------
*/
static const char rcsid[] = "$Id: dynamics.cpp,v 1.34 2006/05/19 18:32:30 gourdeau Exp $";

#include "robot.h"

#ifdef use_namespace
namespace ROBOOP {
  using namespace NEWMAT;
#endif


ReturnMatrix Robot_basic::inertia(const ColumnVector & q)
{
   Matrix M(dof,dof);
   ColumnVector torque(dof);
   set_q(q);
   for(int i = 1; i <= dof; i++) {
      for(int j = 1; j <= dof; j++) {
         torque(j) = (i == j ? 1.0 : 0.0);
      }
      torque = torque_novelocity(torque);
      M.Column(i) = torque;
   }
   M.Release(); return M;
}


ReturnMatrix Robot_basic::acceleration(const ColumnVector & q,
                                       const ColumnVector & qp,
                                       const ColumnVector & tau_cmd)
{
   ColumnVector qpp(dof);
   qpp = 0.0;
   qpp = inertia(q).i()*(tau_cmd-torque(q,qp,qpp));
   qpp.Release(); 
   return qpp;
}

ReturnMatrix Robot_basic::acceleration(const ColumnVector & q, const ColumnVector & qp,
                                       const ColumnVector & tau_cmd, const ColumnVector & Fext,
                                       const ColumnVector & Next)
{
   ColumnVector qpp(dof);
   qpp = 0.0;
   qpp = inertia(q).i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
   qpp.Release(); 
   return qpp;
}

ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
                           const ColumnVector & qpp)
{
   ColumnVector Fext(3), Next(3);
   Fext = 0;
   Next = 0;
   return torque(q, qp, qpp, Fext, Next);
}

ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
                           const ColumnVector & qpp, const ColumnVector & Fext,
                           const ColumnVector & Next)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(q.Nrows() != dof) error("q has wrong dimension");
   if(qp.Nrows() != dof) error("qp has wrong dimension");
   if(qpp.Nrows() != dof) error("qpp has wrong dimension");
   set_q(q);
   set_qp(qp);

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         w[i] = Rt*(w[i-1] + z0*qp(i));
         wp[i] = Rt*(wp[i-1] + z0*qpp(i)
                     + CrossProduct(w[i-1],z0*qp(i)));
         vp[i] = CrossProduct(wp[i],p[i])
                 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
                 + Rt*(vp[i-1]);
      } else {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + z0*qpp(i))
                 + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
                 + CrossProduct(wp[i],p[i])
                 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
      }
      a[i] = CrossProduct(wp[i],links[i].r)
             + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
             + vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
      if(i == dof) {
         f[i] = F[i] + Fext;
         n[i] = CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]) + N[i] + Next;
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]) + N[i];
      }
      if(links[i].get_joint_type() == 0)
         temp = ((z0.t()*links[i].R)*n[i]);
      else
         temp = ((z0.t()*links[i].R)*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix Robot::torque_novelocity(const ColumnVector & qpp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qpp.Nrows() != dof) error("qpp has wrong dimension");

   vp[0] = 0.0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         wp[i] = Rt*(wp[i-1] + z0*qpp(i));
         vp[i] = CrossProduct(wp[i],p[i])
                 + Rt*(vp[i-1]);
      } else {
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + z0*qpp(i))
                 + CrossProduct(wp[i],p[i]);
      }
      a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] = a[i] * links[i].m;
      N[i] = links[i].I*wp[i];
      if(i == dof) {
         f_nv[i] = F[i];
         n_nv[i] = CrossProduct(p[i],f_nv[i])
                   + CrossProduct(links[i].r,F[i]) + N[i];
      } else {
         f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
         n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i],f_nv[i])
                   + CrossProduct(links[i].r,F[i]) + N[i];
      }
      if(links[i].get_joint_type() == 0)
         temp = ((z0.t()*links[i].R)*n_nv[i]);
      else
         temp = ((z0.t()*links[i].R)*f_nv[i]);
      ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);

   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix Robot::G()
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
         vp[i] = Rt*(vp[i-1]);
      else
         vp[i] = Rt*vp[i-1];

      a[i] = vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      if(i == dof) {
         f[i] = F[i];
         n[i] = CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]);
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]);
      }
      if(links[i].get_joint_type() == 0)
         temp = ((z0.t()*links[i].R)*n[i]);
      else
         temp = ((z0.t()*links[i].R)*f[i]);
      ltorque(i) = temp(1,1);
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix Robot::C(const ColumnVector & qp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qp.Nrows() != dof) error("qp has wrong dimension");

   vp[0]=0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         w[i] = Rt*(w[i-1] + z0*qp(i));
         wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
         vp[i] = CrossProduct(wp[i],p[i])
                 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
                 + Rt*(vp[i-1]);
      } else {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*vp[i-1] + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
                 + CrossProduct(wp[i],p[i])
                 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
      }
      a[i] = CrossProduct(wp[i],links[i].r)
             + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
             + vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
      if(i == dof) {
         f[i] = F[i];
         n[i] = CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]) + N[i];
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
                + CrossProduct(links[i].r,F[i]) + N[i];
      }
      if(links[i].get_joint_type() == 0)
         temp = ((z0.t()*links[i].R)*n[i]);
      else
         temp = ((z0.t()*links[i].R)*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
                            const ColumnVector & qpp)
{
   ColumnVector Fext(3), Next(3);
   Fext = 0;
   Next = 0;
   return torque(q, qp, qpp, Fext, Next);
}

ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
                            const ColumnVector & qpp, const ColumnVector & Fext_,
                            const ColumnVector & Next_)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(q.Nrows() != dof) error("q has wrong dimension");
   if(qp.Nrows() != dof) error("qp has wrong dimension");
   if(qpp.Nrows() != dof) error("qpp has wrong dimension");
   set_q(q);
   set_qp(qp);

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         w[i] = Rt*w[i-1] + z0*qp(i);
         wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
                 + z0*qpp(i);
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
                     + vp[i-1]);
      } else {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
                 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
      }
      a[i] = CrossProduct(wp[i],links[i].r)
             + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
             + vp[i];
   }

   // Load on last link
   ColumnVector Fext(3), Next(3);
   if(fix) // Last link is fix
   {
      Fext = links[dof+fix].R*Fext_;
      Next = links[dof+fix].R*Next_;
   }
   else
   {
      Fext = Fext_;
      Next = Next_;
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
      if(i == dof) {
         f[i] = F[i] + Fext;
         n[i] = CrossProduct(links[i].r,F[i]) + N[i] + Next;
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
                + CrossProduct(links[i].r,F[i]) + N[i];
      }
      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot::torque_novelocity(const ColumnVector & qpp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");

   vp[0] = 0.0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         wp[i] = Rt*wp[i-1] + z0*qpp(i);
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
      } else {
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
                 + z0*qpp(i);
      }
      a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      N[i] = links[i].I*wp[i];
      if(i == dof) {
         f_nv[i] = F[i];
         n_nv[i] = CrossProduct(links[i].r,F[i]) + N[i];
      } else {
         f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
         n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
                   + CrossProduct(links[i].r,F[i]) + N[i];
      }

      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n_nv[i]);
      else
         temp = (z0.t()*f_nv[i]);
      ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot::G()
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
         vp[i] = Rt*vp[i-1];
      else
         vp[i] = Rt*vp[i-1];
      a[i] = vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      if(i == dof) {
         f[i] = F[i];
         n[i] = CrossProduct(links[i].r,F[i]);
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
                + CrossProduct(links[i].r,F[i]);
      }
      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1);
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot::C(const ColumnVector & qp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qp.Nrows() != dof) error("qp has wrong dimension");

   vp[0] = 0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0) {
         w[i] = Rt*w[i-1] + z0*qp(i);
         wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
                     + vp[i-1]);
      } else {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
                 +  2.0*CrossProduct(w[i],z0*qp(i));
      }
      a[i] = CrossProduct(wp[i],links[i].r)
             + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
             + vp[i];
   }

   for(i = dof; i >= 1; i--) {
      F[i] =  a[i] * links[i].m;
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
      if(i == dof) {
         f[i] = F[i];
         n[i] = CrossProduct(links[i].r,F[i]) + N[i];
      } else {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
                + CrossProduct(links[i].r,F[i]) + N[i];
      }
      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
                                     const ColumnVector & qpp)
{
   ColumnVector Fext(3), Next(3);
   Fext = 0;
   Next = 0;
   return torque(q, qp, qpp, Fext, Next);
}

ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
                                     const ColumnVector & qpp, const ColumnVector & Fext_,
                                     const ColumnVector & Next_)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(q.Nrows() != dof) error("q has wrong dimension");
   if(qp.Nrows() != dof) error("qp has wrong dimension");
   if(qpp.Nrows() != dof) error("qpp has wrong dimension");
   set_q(q);
   set_qp(qp);

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
      {
         w[i] = Rt*w[i-1] + z0*qp(i);
         wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)))
                 + z0*qpp(i);
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
                     + vp[i-1]);
      }
      else
      {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
                 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
      }
   }

   ColumnVector Fext(3), Next(3);
   if(fix)
   {
      Fext = links[dof+fix].R*Fext_;
      Next = links[dof+fix].R*Next_;
   }
   else
   {
      Fext = Fext_;
      Next = Next_;
   }

   for(i = dof; i >= 1; i--)
   {
      F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
             CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
             CrossProduct(-vp[i], links[i].mc);
      if(i == dof)
      {
         f[i] = F[i] + Fext;
         n[i] = N[i] + Next;
      }
      else
      {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
      }

      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}


ReturnMatrix mRobot_min_para::torque_novelocity(const ColumnVector & qpp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");

   vp[0] = 0.0;
   for(i = 1; i <= dof; i++)
   {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
      {
         wp[i] = Rt*wp[i-1] + z0*qpp(i);
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
      }
      else
      {
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
                 + z0*qpp(i);
      }
   }

   for(i = dof; i >= 1; i--)
   {
      F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc);
      N[i] = links[i].I*wp[i] + CrossProduct(-vp[i], links[i].mc);
      if(i == dof)
      {
         f_nv[i] = F[i];
         n_nv[i] = N[i];
      }
      else
      {
         f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
         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];
      }

      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n_nv[i]);
      else
         temp = (z0.t()*f_nv[i]);
      ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot_min_para::G()
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;

   vp[0] = gravity;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
         vp[i] = Rt*vp[i-1];
      else
         vp[i] = Rt*vp[i-1];
   }

   for(i = dof; i >= 1; i--)
   {
      F[i] = vp[i]*links[i].m;
      N[i] = CrossProduct(-vp[i], links[i].mc);
      if(i == dof)
      {
         f[i] = F[i];
         n[i] = N[i];
      }
      else
      {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
      }

      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1);
   }

   ltorque.Release(); return ltorque;
}

ReturnMatrix mRobot_min_para::C(const ColumnVector & qp)
{
   int i;
   ColumnVector ltorque(dof);
   Matrix Rt, temp;
   if(qp.Nrows() != dof) error("qp has wrong dimension");
   set_qp(qp);

   vp[0] = 0;
   for(i = 1; i <= dof; i++) {
      Rt = links[i].R.t();
      if(links[i].get_joint_type() == 0)
      {
         w[i] = Rt*w[i-1] + z0*qp(i);
         wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
         vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
                     + vp[i-1]);
      }
      else
      {
         w[i] = Rt*w[i-1];
         wp[i] = Rt*wp[i-1];
         vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
                     + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
                 + 2.0*CrossProduct(w[i],z0*qp(i));
      }
   }

   for(i = dof; i >= 1; i--)
   {
      F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
             CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
      N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
             CrossProduct(-vp[i], links[i].mc);
      if(i == dof)
      {
         f[i] = F[i];
         n[i] = N[i];
      }
      else
      {
         f[i] = links[i+1].R*f[i+1] + F[i];
         n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
      }

      if(links[i].get_joint_type() == 0)
         temp = (z0.t()*n[i]);
      else
         temp = (z0.t()*f[i]);
      ltorque(i) = temp(1,1)
                   + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
   }

   ltorque.Release(); return ltorque;
}

#ifdef use_namespace
}
#endif