Homepage Demos Overview Downloads Tutorials Reference
Credits

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 */
00064 /*!
00065   @file dynamics.cpp
00066   @brief Manipulator dynamics functions.
00067 */
00068 
00069 //! @brief RCS/CVS version.
00070 static const char rcsid[] = "$Id: dynamics.cpp,v 1.6 2004/07/14 03:22:32 ejt Exp $";
00071 
00072 #include "robot.h"
00073 
00074 #ifdef use_namespace
00075 namespace ROBOOP {
00076   using namespace NEWMAT;
00077 #endif
00078 
00079 ReturnMatrix Robot_basic::inertia(const ColumnVector & q)
00080 //! @brief Inertia of the manipulator.
00081 {
00082    Matrix M(dof,dof);
00083    ColumnVector torque(dof);
00084    set_q(q);
00085    for(int i = 1; i <= dof; i++) {
00086       for(int j = 1; j <= dof; j++) {
00087          torque(j) = (i == j ? 1.0 : 0.0);
00088       }
00089       torque = torque_novelocity(torque);
00090       M.Column(i) = torque;
00091    }
00092    M.Release(); return M;
00093 }
00094 
00095 
00096 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q,
00097                                        const ColumnVector & qp,
00098                                        const ColumnVector & tau_cmd)
00099 //! @brief Joints acceleration without contact force.
00100 {
00101    ColumnVector qpp(dof);
00102    qpp = 0.0;
00103    qpp = inertia(q).i()*(tau_cmd-torque(q,qp,qpp));
00104    qpp.Release(); 
00105    return qpp;
00106 }
00107 
00108 ReturnMatrix Robot_basic::acceleration(const ColumnVector & q, const ColumnVector & qp,
00109                                        const ColumnVector & tau_cmd, const ColumnVector & Fext,
00110                                        const ColumnVector & Next)
00111 /*!
00112   @brief Joints acceleration.
00113 
00114   The robot dynamics is 
00115   \f[
00116     B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f
00117   \f]
00118   then the joint acceleration is
00119   \f[
00120    \ddot{q} = B^{-1}(q)\big(\tau - J^T(q)f - C(q,\dot{q})\dot{q} - D\dot{q} - g(q)\big ) 
00121   \f]
00122 */
00123 {
00124    ColumnVector qpp(dof);
00125    qpp = 0.0;
00126    qpp = inertia(q).i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
00127    qpp.Release(); 
00128    return qpp;
00129 }
00130 
00131 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00132                            const ColumnVector & qpp)
00133 /*!
00134   @brief Joint torque, without contact force, based on Recursive 
00135   Newton-Euler formulation.
00136 */
00137 {
00138    ColumnVector Fext(3), Next(3);
00139    Fext = 0;
00140    Next = 0;
00141    return torque(q, qp, qpp, Fext, Next);
00142 }
00143 
00144 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
00145                            const ColumnVector & qpp, const ColumnVector & Fext,
00146                            const ColumnVector & Next)
00147 /*!
00148   @brief Joint torque based on Recursive Newton-Euler formulation.
00149 
00150 
00151   In order to apply the RNE as presented in Murray 86, 
00152   let us define the following variables 
00153   (referenced in the \f$i^{th}\f$ coordinate frame if applicable):
00154 
00155   \f$\sigma_i\f$ is the joint type; \f$\sigma_i = 1\f$ for a revolute
00156   joint and \f$\sigma_i = 0\f$ for a prismatic joint.
00157 
00158   \f$ z_0 = 
00159     \left [
00160       \begin{array}{ccc}
00161          0 & 0 & 1
00162       \end{array}
00163     \right ]^T\f$
00164 
00165   \f$p_i = 
00166     \left [
00167       \begin{array}{ccc}
00168       a_i & d_i \sin \alpha_i & d_i \cos \alpha_i
00169       \end{array}
00170     \right ]^T\f$ is the position of the \f$i^{th}\f$ with respect to the \f$i-1^{th}\f$ frame.
00171 
00172     Forward Iterations for \f$i=1, 2, \ldots, n\f$. 
00173     Initialize: \f$\omega_0 = \dot{\omega}_0 = 0\f$ and \f$\dot{v}_0 = - g\f$.
00174     \f[
00175     \omega_i = R_i^T [\omega_{i-1} + \sigma_i z_0 \dot{\theta}_i ] 
00176     \f]
00177     \f[
00178     \dot{\omega}_i = R_i^T  \{ \dot{\omega}_{i-1} + 
00179     \sigma_i [z_0 \ddot{\theta}_i + \omega_{i-1} \times (z_0 \dot{\theta}_i )] \} 
00180     \f]
00181     \f[
00182     \dot{v}_i = R_i^T  \{ \dot{v}_{i-1} + 
00183     (1 -\sigma_i) [z_0 \ddot{d}_i + 2 \omega_{i-1} \times (z_0 \dot{d}_i )] \} 
00184     + \dot{\omega}_i \times p_i + \omega_i \times ( \omega_i \times p_i)
00185     \f]
00186 
00187     Backward Iterations for \f$i=n, n-1, \ldots, 1\f$. 
00188     Initialize: $f_{n+1} = n_{n+1} = 0$.
00189     \f[
00190     \dot{v}_{ci} = v_i + \omega_i \times r_i 
00191     + \omega_i \times (\omega_i \times r_i) 
00192     \f]
00193     \f[
00194     F_i = m_i \dot{v}_{ci} 
00195     \f]
00196     \f[
00197     N_i = I_{ci} \dot{\omega}_i + \omega_i \times (I_{ci} \omega_i)
00198     \f]
00199     \f[
00200     f_i = R_{i+1} [ f_{i+1} ] + F_{i} 
00201     \f]
00202     \f[
00203     n_i = R_{i+1} [ n_{i+1} ]  + p_{i} \times f_{i} 
00204     + N_{i} + r_{i} \times F_{i}
00205     \f]
00206     \f[
00207     \tau_i = \sigma_i n_i^T (R_i^T z_0) 
00208     + (1 - \sigma_i) f_i^T (R_i^T z_0)
00209     \f]
00210 */
00211 {
00212    int i;
00213    ColumnVector ltorque(dof);
00214    Matrix Rt, temp;
00215    if(q.Nrows() != dof) error("q has wrong dimension");
00216    if(qp.Nrows() != dof) error("qp has wrong dimension");
00217    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00218    set_q(q);
00219    set_qp(qp);
00220 
00221    vp[0] = gravity;
00222    for(i = 1; i <= dof; i++) {
00223       Rt = links[i].R.t();
00224       if(links[i].get_joint_type() == 0) {
00225          w[i] = Rt*(w[i-1] + z0*qp(i));
00226          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00227                      + CrossProduct(w[i-1],z0*qp(i)));
00228          vp[i] = CrossProduct(wp[i],p[i])
00229                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00230                  + Rt*(vp[i-1]);
00231       } else {
00232          w[i] = Rt*w[i-1];
00233          wp[i] = Rt*wp[i-1];
00234          vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00235                  + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00236                  + CrossProduct(wp[i],p[i])
00237                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00238       }
00239       a[i] = CrossProduct(wp[i],links[i].r)
00240              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00241              + vp[i];
00242    }
00243 
00244    for(i = dof; i >= 1; i--) {
00245       F[i] =  a[i] * links[i].m;
00246       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00247       if(i == dof) {
00248          f[i] = F[i] + Fext;
00249          n[i] = CrossProduct(p[i],f[i])
00250                 + CrossProduct(links[i].r,F[i]) + N[i] + Next;
00251       } else {
00252          f[i] = links[i+1].R*f[i+1] + F[i];
00253          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00254                 + CrossProduct(links[i].r,F[i]) + N[i];
00255       }
00256       if(links[i].get_joint_type() == 0)
00257          temp = ((z0.t()*links[i].R)*n[i]);
00258       else
00259          temp = ((z0.t()*links[i].R)*f[i]);
00260       ltorque(i) = temp(1,1)
00261                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00262                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00263    }
00264 
00265    ltorque.Release(); return ltorque;
00266 }
00267 
00268 ReturnMatrix Robot::torque_novelocity(const ColumnVector & qpp)
00269 /*!
00270   @brief Joint torque. when joint velocity is 0, based on Recursive 
00271   Newton-Euler formulation.
00272 */
00273 {
00274    int i;
00275    ColumnVector ltorque(dof);
00276    Matrix Rt, temp;
00277    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00278 
00279    vp[0] = 0.0;
00280    for(i = 1; i <= dof; i++) {
00281       Rt = links[i].R.t();
00282       if(links[i].get_joint_type() == 0) {
00283          wp[i] = Rt*(wp[i-1] + z0*qpp(i));
00284          vp[i] = CrossProduct(wp[i],p[i])
00285                  + Rt*(vp[i-1]);
00286       } else {
00287          wp[i] = Rt*wp[i-1];
00288          vp[i] = Rt*(vp[i-1] + z0*qpp(i))
00289                  + CrossProduct(wp[i],p[i]);
00290       }
00291       a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00292    }
00293 
00294    for(i = dof; i >= 1; i--) {
00295       F[i] = a[i] * links[i].m;
00296       N[i] = links[i].I*wp[i];
00297       if(i == dof) {
00298          f_nv[i] = F[i];
00299          n_nv[i] = CrossProduct(p[i],f_nv[i])
00300                    + CrossProduct(links[i].r,F[i]) + N[i];
00301       } else {
00302          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00303          n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i],f_nv[i])
00304                    + CrossProduct(links[i].r,F[i]) + N[i];
00305       }
00306       if(links[i].get_joint_type() == 0)
00307          temp = ((z0.t()*links[i].R)*n_nv[i]);
00308       else
00309          temp = ((z0.t()*links[i].R)*f_nv[i]);
00310       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00311 
00312    }
00313 
00314    ltorque.Release(); return ltorque;
00315 }
00316 
00317 ReturnMatrix Robot::G()
00318 //! @brief Joint torque due to gravity based on Recursive Newton-Euler formulation.
00319 {
00320    int i;
00321    ColumnVector ltorque(dof);
00322    Matrix Rt, temp;
00323 
00324    vp[0] = gravity;
00325    for(i = 1; i <= dof; i++) {
00326       Rt = links[i].R.t();
00327       if(links[i].get_joint_type() == 0)
00328          vp[i] = Rt*(vp[i-1]);
00329       else
00330          vp[i] = Rt*vp[i-1];
00331 
00332       a[i] = vp[i];
00333    }
00334 
00335    for(i = dof; i >= 1; i--) {
00336       F[i] =  a[i] * links[i].m;
00337       if(i == dof) {
00338          f[i] = F[i];
00339          n[i] = CrossProduct(p[i],f[i])
00340                 + CrossProduct(links[i].r,F[i]);
00341       } else {
00342          f[i] = links[i+1].R*f[i+1] + F[i];
00343          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00344                 + CrossProduct(links[i].r,F[i]);
00345       }
00346       if(links[i].get_joint_type() == 0)
00347          temp = ((z0.t()*links[i].R)*n[i]);
00348       else
00349          temp = ((z0.t()*links[i].R)*f[i]);
00350       ltorque(i) = temp(1,1);
00351    }
00352 
00353    ltorque.Release(); return ltorque;
00354 }
00355 
00356 ReturnMatrix Robot::C(const ColumnVector & qp)
00357 //! @brief Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
00358 {
00359    int i;
00360    ColumnVector ltorque(dof);
00361    Matrix Rt, temp;
00362    if(qp.Nrows() != dof) error("qp has wrong dimension");
00363 
00364    vp[0]=0;
00365    for(i = 1; i <= dof; i++) {
00366       Rt = links[i].R.t();
00367       if(links[i].get_joint_type() == 0) {
00368          w[i] = Rt*(w[i-1] + z0*qp(i));
00369          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00370          vp[i] = CrossProduct(wp[i],p[i])
00371                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00372                  + Rt*(vp[i-1]);
00373       } else {
00374          w[i] = Rt*w[i-1];
00375          wp[i] = Rt*wp[i-1];
00376          vp[i] = Rt*vp[i-1] + 2.0*CrossProduct(w[i],Rt*z0*qp(i))
00377                  + CrossProduct(wp[i],p[i])
00378                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00379       }
00380       a[i] = CrossProduct(wp[i],links[i].r)
00381              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00382              + vp[i];
00383    }
00384 
00385    for(i = dof; i >= 1; i--) {
00386       F[i] =  a[i] * links[i].m;
00387       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00388       if(i == dof) {
00389          f[i] = F[i];
00390          n[i] = CrossProduct(p[i],f[i])
00391                 + CrossProduct(links[i].r,F[i]) + N[i];
00392       } else {
00393          f[i] = links[i+1].R*f[i+1] + F[i];
00394          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00395                 + CrossProduct(links[i].r,F[i]) + N[i];
00396       }
00397       if(links[i].get_joint_type() == 0)
00398          temp = ((z0.t()*links[i].R)*n[i]);
00399       else
00400          temp = ((z0.t()*links[i].R)*f[i]);
00401       ltorque(i) = temp(1,1)
00402                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00403    }
00404 
00405    ltorque.Release(); return ltorque;
00406 }
00407 
00408 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00409                             const ColumnVector & qpp)
00410 //! @brief Joint torque, without contact force, based on Recursive Newton-Euler formulation.
00411 {
00412    ColumnVector Fext(3), Next(3);
00413    Fext = 0;
00414    Next = 0;
00415    return torque(q, qp, qpp, Fext, Next);
00416 }
00417 
00418 ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00419                             const ColumnVector & qpp, const ColumnVector & Fext_,
00420                             const ColumnVector & Next_)
00421 /*!
00422   @brief Joint torque based on Recursive Newton-Euler formulation.
00423 
00424 
00425   In order to apply the RNE, let us define the following variables 
00426   (referenced in the \f$i^{th}\f$ coordinate frame if applicable):
00427 
00428   \f$\sigma_i\f$ is the joint type; \f$\sigma_i = 1\f$ for a revolute
00429   joint and \f$\sigma_i = 0\f$ for a prismatic joint.
00430 
00431   \f$ z_0 = 
00432     \left [
00433       \begin{array}{ccc}
00434          0 & 0 & 1
00435       \end{array}
00436     \right ]^T\f$
00437 
00438   \f$p_i =
00439     \left [
00440       \begin{array}{ccc}
00441       a_{i-1} & -d_i sin \alpha_{i-1} & d_i cos \alpha_{i-1}
00442       \end{array}
00443     \right ]^T\f$ is the position of the $i^{th}$ with respect to the $i-1^{th}$ frame.
00444 
00445   Forward Iterations for \f$i=1, 2, \ldots, n\f$.  Initialize: 
00446   \f$\omega_0 = \dot{\omega}_0 = 0$ and $\dot{v}_0 = - g\f$. 
00447 
00448   \f[
00449   \omega_i = R_i^T\omega_{i-1} + \sigma_i z_0\dot{\theta_i} 
00450   \f]
00451   \f[
00452   \dot{\omega}_i = R_i^T\dot{\omega}_{i-1} + \sigma_i
00453   R_i^T\omega_{i-1}\times z_0 \dot{\theta}_i + \sigma_iz_0\ddot{\theta}_i
00454   \f]
00455   \f[
00456   \dot{v}_i = R_i^T(\dot{\omega}_{i-1}\times p_i +
00457   \omega_{i-1}\times(\omega_{i-1}\times p_i) + \dot{v}_{i-1})
00458   + (1 - \sigma_i)(2\omega_i\times z_0 dot{d}_i + z_0\ddot{d}_i)
00459   \f]
00460 
00461   Backward Iterations for \f$i=n, n-1, \ldots, 1\f$. Initialize: \f$f_{n+1} = n_{n+1} = 0\f$.
00462 
00463   \f[
00464   \dot{v}_{ci} = \dot{\omega}_i\times r_i + 
00465   \omega_i\times(\omega_i\times r_i) + v_i 
00466   \f]
00467   \f[
00468   F_i = m_i \dot{v}_{ci} 
00469   \f]
00470   \f[
00471   N_i = I_{ci}\ddot{\omega}_i\ + \omega_i \times I_{ci}\omega_i 
00472   \f]
00473   \f[
00474   f_i = R_{i+1}f_{i+1} + F_i
00475   \f]
00476   \f[
00477   n_i = N_i + R_{i+1} n_{i+1} + r_i \times F_i + p_{i+1}\times R_{i+1}f_{i+1}
00478   \f]
00479   \f[
00480   \tau_i = \sigma_i n_i^T z_0 + (1 - \sigma_i) f_i^T z_0
00481   \f]
00482 */
00483 {
00484    int i;
00485    ColumnVector ltorque(dof);
00486    Matrix Rt, temp;
00487    if(q.Nrows() != dof) error("q has wrong dimension");
00488    if(qp.Nrows() != dof) error("qp has wrong dimension");
00489    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00490    set_q(q);
00491    set_qp(qp);
00492 
00493    vp[0] = gravity;
00494    for(i = 1; i <= dof; i++) {
00495       Rt = links[i].R.t();
00496       if(links[i].get_joint_type() == 0) {
00497          w[i] = Rt*w[i-1] + z0*qp(i);
00498          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00499                  + z0*qpp(i);
00500          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00501                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00502                      + vp[i-1]);
00503       } else {
00504          w[i] = Rt*w[i-1];
00505          wp[i] = Rt*wp[i-1];
00506          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00507                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00508                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00509       }
00510       a[i] = CrossProduct(wp[i],links[i].r)
00511              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00512              + vp[i];
00513    }
00514 
00515    // Load on last link
00516    ColumnVector Fext(3), Next(3);
00517    if(fix) // Last link is fix
00518    {
00519       Fext = links[dof+fix].R*Fext_;
00520       Next = links[dof+fix].R*Next_;
00521    }
00522    else
00523    {
00524       Fext = Fext_;
00525       Next = Next_;
00526    }
00527 
00528    for(i = dof; i >= 1; i--) {
00529       F[i] =  a[i] * links[i].m;
00530       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00531       if(i == dof) {
00532          f[i] = F[i] + Fext;
00533          n[i] = CrossProduct(links[i].r,F[i]) + N[i] + Next;
00534       } else {
00535          f[i] = links[i+1].R*f[i+1] + F[i];
00536          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00537                 + CrossProduct(links[i].r,F[i]) + N[i];
00538       }
00539       if(links[i].get_joint_type() == 0)
00540          temp = (z0.t()*n[i]);
00541       else
00542          temp = (z0.t()*f[i]);
00543       ltorque(i) = temp(1,1)
00544                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00545                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00546    }
00547 
00548    ltorque.Release(); return ltorque;
00549 }
00550 
00551 ReturnMatrix mRobot::torque_novelocity(const ColumnVector & qpp)
00552 //! @brief Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
00553 {
00554    int i;
00555    ColumnVector ltorque(dof);
00556    Matrix Rt, temp;
00557    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00558 
00559    vp[0] = 0.0;
00560    for(i = 1; i <= dof; i++) {
00561       Rt = links[i].R.t();
00562       if(links[i].get_joint_type() == 0) {
00563          wp[i] = Rt*wp[i-1] + z0*qpp(i);
00564          vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00565       } else {
00566          wp[i] = Rt*wp[i-1];
00567          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00568                  + z0*qpp(i);
00569       }
00570       a[i] = CrossProduct(wp[i],links[i].r) + vp[i];
00571    }
00572 
00573    for(i = dof; i >= 1; i--) {
00574       F[i] =  a[i] * links[i].m;
00575       N[i] = links[i].I*wp[i];
00576       if(i == dof) {
00577          f_nv[i] = F[i];
00578          n_nv[i] = CrossProduct(links[i].r,F[i]) + N[i];
00579       } else {
00580          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00581          n_nv[i] = links[i+1].R*n_nv[i+1] + CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
00582                    + CrossProduct(links[i].r,F[i]) + N[i];
00583       }
00584 
00585       if(links[i].get_joint_type() == 0)
00586          temp = (z0.t()*n_nv[i]);
00587       else
00588          temp = (z0.t()*f_nv[i]);
00589       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00590    }
00591 
00592    ltorque.Release(); return ltorque;
00593 }
00594 
00595 ReturnMatrix mRobot::G()
00596 //! @brief Joint torque due to gravity based on Recursive Newton-Euler formulation.
00597 {
00598    int i;
00599    ColumnVector ltorque(dof);
00600    Matrix Rt, temp;
00601 
00602    vp[0] = gravity;
00603    for(i = 1; i <= dof; i++) {
00604       Rt = links[i].R.t();
00605       if(links[i].get_joint_type() == 0)
00606          vp[i] = Rt*vp[i-1];
00607       else
00608          vp[i] = Rt*vp[i-1];
00609       a[i] = vp[i];
00610    }
00611 
00612    for(i = dof; i >= 1; i--) {
00613       F[i] =  a[i] * links[i].m;
00614       if(i == dof) {
00615          f[i] = F[i];
00616          n[i] = CrossProduct(links[i].r,F[i]);
00617       } else {
00618          f[i] = links[i+1].R*f[i+1] + F[i];
00619          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00620                 + CrossProduct(links[i].r,F[i]);
00621       }
00622       if(links[i].get_joint_type() == 0)
00623          temp = (z0.t()*n[i]);
00624       else
00625          temp = (z0.t()*f[i]);
00626       ltorque(i) = temp(1,1);
00627    }
00628 
00629    ltorque.Release(); return ltorque;
00630 }
00631 
00632 ReturnMatrix mRobot::C(const ColumnVector & qp)
00633 //! @brief Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.  
00634 {
00635    int i;
00636    ColumnVector ltorque(dof);
00637    Matrix Rt, temp;
00638    if(qp.Nrows() != dof) error("qp has wrong dimension");
00639 
00640    vp[0] = 0;
00641    for(i = 1; i <= dof; i++) {
00642       Rt = links[i].R.t();
00643       if(links[i].get_joint_type() == 0) {
00644          w[i] = Rt*w[i-1] + z0*qp(i);
00645          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i));
00646          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00647                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00648                      + vp[i-1]);
00649       } else {
00650          w[i] = Rt*w[i-1];
00651          wp[i] = Rt*wp[i-1];
00652          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00653                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00654                  +  2.0*CrossProduct(w[i],z0*qp(i));
00655       }
00656       a[i] = CrossProduct(wp[i],links[i].r)
00657              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00658              + vp[i];
00659    }
00660 
00661    for(i = dof; i >= 1; i--) {
00662       F[i] =  a[i] * links[i].m;
00663       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00664       if(i == dof) {
00665          f[i] = F[i];
00666          n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00667       } else {
00668          f[i] = links[i+1].R*f[i+1] + F[i];
00669          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00670                 + CrossProduct(links[i].r,F[i]) + N[i];
00671       }
00672       if(links[i].get_joint_type() == 0)
00673          temp = (z0.t()*n[i]);
00674       else
00675          temp = (z0.t()*f[i]);
00676       ltorque(i) = temp(1,1)
00677                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00678    }
00679 
00680    ltorque.Release(); return ltorque;
00681 }
00682 
00683 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00684                                      const ColumnVector & qpp)
00685 //! @brief Joint torque without contact force based on Recursive Newton-Euler formulation.
00686 {
00687    ColumnVector Fext(3), Next(3);
00688    Fext = 0;
00689    Next = 0;
00690    return torque(q, qp, qpp, Fext, Next);
00691 }
00692 
00693 ReturnMatrix mRobot_min_para::torque(const ColumnVector & q, const ColumnVector & qp,
00694                                      const ColumnVector & qpp, const ColumnVector & Fext_,
00695                                      const ColumnVector & Next_)
00696 /*!
00697   @brief Joint torque based on Recursive Newton-Euler formulation.
00698 
00699   See ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp,
00700                                   const ColumnVector & qpp, const ColumnVector & Fext,
00701                                   const ColumnVector & Next)
00702   for the Recursive Newton-Euler formulation.
00703 */
00704 {
00705    int i;
00706    ColumnVector ltorque(dof);
00707    Matrix Rt, temp;
00708    if(q.Nrows() != dof) error("q has wrong dimension");
00709    if(qp.Nrows() != dof) error("qp has wrong dimension");
00710    if(qpp.Nrows() != dof) error("qpp has wrong dimension");
00711    set_q(q);
00712    set_qp(qp);
00713 
00714    vp[0] = gravity;
00715    for(i = 1; i <= dof; i++) {
00716       Rt = links[i].R.t();
00717       if(links[i].get_joint_type() == 0)
00718       {
00719          w[i] = Rt*w[i-1] + z0*qp(i);
00720          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)))
00721                  + z0*qpp(i);
00722          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00723                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00724                      + vp[i-1]);
00725       }
00726       else
00727       {
00728          w[i] = Rt*w[i-1];
00729          wp[i] = Rt*wp[i-1];
00730          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00731                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00732                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00733       }
00734    }
00735 
00736    ColumnVector Fext(3), Next(3);
00737    if(fix)
00738    {
00739       Fext = links[dof+fix].R*Fext_;
00740       Next = links[dof+fix].R*Next_;
00741    }
00742    else
00743    {
00744       Fext = Fext_;
00745       Next = Next_;
00746    }
00747 
00748    for(i = dof; i >= 1; i--)
00749    {
00750       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00751              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00752       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00753              CrossProduct(-vp[i], links[i].mc);
00754       if(i == dof)
00755       {
00756          f[i] = F[i] + Fext;
00757          n[i] = N[i] + Next;
00758       }
00759       else
00760       {
00761          f[i] = links[i+1].R*f[i+1] + F[i];
00762          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00763       }
00764 
00765       if(links[i].get_joint_type() == 0)
00766          temp = (z0.t()*n[i]);
00767       else
00768          temp = (z0.t()*f[i]);
00769       ltorque(i) = temp(1,1)
00770                    + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
00771                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00772    }
00773 
00774    ltorque.Release(); return ltorque;
00775 }
00776 
00777 
00778 ReturnMatrix mRobot_min_para::torque_novelocity(const ColumnVector & qpp)
00779 //! @brief Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
00780 {
00781    int i;
00782    ColumnVector ltorque(dof);
00783    Matrix Rt, temp;
00784    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00785 
00786    vp[0] = 0.0;
00787    for(i = 1; i <= dof; i++)
00788    {
00789       Rt = links[i].R.t();
00790       if(links[i].get_joint_type() == 0)
00791       {
00792          wp[i] = Rt*wp[i-1] + z0*qpp(i);
00793          vp[i] = Rt*(CrossProduct(wp[i-1],p[i]) + vp[i-1]);
00794       }
00795       else
00796       {
00797          wp[i] = Rt*wp[i-1];
00798          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i]))
00799                  + z0*qpp(i);
00800       }
00801    }
00802 
00803    for(i = dof; i >= 1; i--)
00804    {
00805       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc);
00806       N[i] = links[i].I*wp[i] + CrossProduct(-vp[i], links[i].mc);
00807       if(i == dof)
00808       {
00809          f_nv[i] = F[i];
00810          n_nv[i] = N[i];
00811       }
00812       else
00813       {
00814          f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
00815          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];
00816       }
00817 
00818       if(links[i].get_joint_type() == 0)
00819          temp = (z0.t()*n_nv[i]);
00820       else
00821          temp = (z0.t()*f_nv[i]);
00822       ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
00823    }
00824 
00825    ltorque.Release(); return ltorque;
00826 }
00827 
00828 ReturnMatrix mRobot_min_para::G()
00829 //! @brief Joint torque due to gravity based on Recursive Newton-Euler formulation.
00830 {
00831    int i;
00832    ColumnVector ltorque(dof);
00833    Matrix Rt, temp;
00834 
00835    vp[0] = gravity;
00836    for(i = 1; i <= dof; i++) {
00837       Rt = links[i].R.t();
00838       if(links[i].get_joint_type() == 0)
00839          vp[i] = Rt*vp[i-1];
00840       else
00841          vp[i] = Rt*vp[i-1];
00842    }
00843 
00844    for(i = dof; i >= 1; i--)
00845    {
00846       F[i] = vp[i]*links[i].m;
00847       N[i] = CrossProduct(-vp[i], links[i].mc);
00848       if(i == dof)
00849       {
00850          f[i] = F[i];
00851          n[i] = N[i];
00852       }
00853       else
00854       {
00855          f[i] = links[i+1].R*f[i+1] + F[i];
00856          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00857       }
00858 
00859       if(links[i].get_joint_type() == 0)
00860          temp = (z0.t()*n[i]);
00861       else
00862          temp = (z0.t()*f[i]);
00863       ltorque(i) = temp(1,1);
00864    }
00865 
00866    ltorque.Release(); return ltorque;
00867 }
00868 
00869 ReturnMatrix mRobot_min_para::C(const ColumnVector & qp)
00870 //! @brief Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.  
00871 {
00872    int i;
00873    ColumnVector ltorque(dof);
00874    Matrix Rt, temp;
00875    if(qp.Nrows() != dof) error("qp has wrong dimension");
00876    set_qp(qp);
00877 
00878    vp[0] = 0;
00879    for(i = 1; i <= dof; i++) {
00880       Rt = links[i].R.t();
00881       if(links[i].get_joint_type() == 0)
00882       {
00883          w[i] = Rt*w[i-1] + z0*qp(i);
00884          wp[i] = Rt*(wp[i-1] + CrossProduct(w[i-1],z0*qp(i)));
00885          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00886                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00887                      + vp[i-1]);
00888       }
00889       else
00890       {
00891          w[i] = Rt*w[i-1];
00892          wp[i] = Rt*wp[i-1];
00893          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00894                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00895                  + 2.0*CrossProduct(w[i],z0*qp(i));
00896       }
00897    }
00898 
00899    for(i = dof; i >= 1; i--)
00900    {
00901       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00902              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00903       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]) +
00904              CrossProduct(-vp[i], links[i].mc);
00905       if(i == dof)
00906       {
00907          f[i] = F[i];
00908          n[i] = N[i];
00909       }
00910       else
00911       {
00912          f[i] = links[i+1].R*f[i+1] + F[i];
00913          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
00914       }
00915 
00916       if(links[i].get_joint_type() == 0)
00917          temp = (z0.t()*n[i]);
00918       else
00919          temp = (z0.t()*f[i]);
00920       ltorque(i) = temp(1,1)
00921                    + links[i].Gr*(links[i].B*qp(i) + links[i].Cf*sign(qp(i)));
00922    }
00923 
00924    ltorque.Release(); return ltorque;
00925 }
00926 
00927 #ifdef use_namespace
00928 }
00929 #endif
00930 

ROBOOP v1.21a
Generated Tue Oct 19 14:18:25 2004 by Doxygen 1.3.9.1