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

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4