Homepage Demos Overview Downloads Tutorials Reference
Credits

delta_t.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 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2003/29/04: Etienne Lachance
00035    -Fix Robot::delta_torque.
00036    -Added mRobot/mRobot_min_para::delta_torque.
00037 
00038 2004/05/14: Etienne Lachance
00039    -Replaced vec_x_prod by CrossProduct.
00040 
00041 2004/07/01: Ethan Tira-Thompson
00042     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00043 
00044 2004/07/02: Etienne Lachance
00045    -Added Doxygen comments.
00046 -------------------------------------------------------------------------------
00047 */
00048 
00049 /*!
00050   @file delta_t.cpp
00051   @brief Delta torque (linearized dynamics).
00052 */
00053 
00054 #include "robot.h"
00055 
00056 #ifdef use_namespace
00057 namespace ROBOOP {
00058   using namespace NEWMAT;
00059 #endif
00060   //! @brief RCS/CVS version.
00061   static const char rcsid[] __UNUSED__ = "$Id: delta_t.cpp,v 1.6 2005/07/26 03:22:09 ejt Exp $";
00062 
00063 void Robot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00064                          const ColumnVector & qpp, const ColumnVector & dq,
00065                          const ColumnVector & dqp, const ColumnVector & dqpp,
00066                          ColumnVector & ltorque, ColumnVector & dtorque)
00067 /*!
00068   @brief Delta torque dynamics.
00069 
00070   This function computes
00071   \f[
00072     \delta \tau = D(q) \delta \ddot{q}
00073     + S_1(q,\dot{q}) \delta \dot{q} + S_2(q,\dot{q},\ddot{q}) \delta q    
00074   \f]
00075 
00076   Murray and Neuman Cite_: Murray86 have developed an efficient recursive 
00077   linearized Newton-Euler formulation.   In order to apply the RNE as presented in 
00078   let us define the following variables 
00079 
00080   \f[
00081     p_{di} = \frac{\partial p_i}{\partial d_i} = 
00082     \left [
00083       \begin{array}{ccc}
00084         0 & \sin \alpha_i & \cos \alpha_i 
00085       \end{array}
00086     \right ]^T
00087   \f]
00088   \f[
00089     Q = 
00090     \left [
00091       \begin{array}{ccc}
00092         0 & -1 & 0  \\
00093   1 & 0 & 0  \\
00094   0 & 0 & 0 
00095       \end{array}
00096   \right ]
00097   \f]
00098 
00099   Forward Iterations for \f$i=1, 2, \ldots, n\f$. 
00100   Initialize: \f$\delta \omega_0 = \delta \dot{\omega}_0 = \delta \dot{v}_0 = 0\f$.
00101   \f[
00102     \delta \omega_i = R_i^T \{\delta \omega_{i-1} + \sigma_i [ z_0 \delta \dot{\theta}_i 
00103     - Q(\omega_{i-1} + \dot{\theta}_i ) \delta \theta_i ] \} 
00104   \f]
00105   \f[
00106   \delta \dot{\omega}_i = R_i^T  \{ \delta \dot{\omega}_{i-1} + 
00107   \sigma_i [z_0 \delta \ddot{\theta}_i + \delta \omega_{i-1} \times (z_0 \dot{\theta}_i )
00108   + \omega_{i-1} \times (z_0 \delta \dot{\theta}_i )] 
00109   - \sigma_i Q [ \omega_{i-1} + z_0 \ddot{\theta}_i 
00110   + \omega_{i-1} \times (z_0 \dot{\theta}_i )] \delta \theta_i \}
00111   \f]
00112 
00113   \f[
00114    \delta \dot{v}_i = R_i^T  \{ \delta \dot{v}_{i-1} - \sigma_i Q \dot{v}_{i-1} \delta \theta_i
00115    + (1 -\sigma_i) [z_0 \delta \ddot{d}_i + 2 \delta \omega_{i-1} \times (z_0 \dot{d}_i ) 
00116    + 2 \omega_{i-1} \times (z_0 \delta \dot{d}_i )] \} + \delta \dot{\omega}_i \times p_i 
00117    + \delta \omega_i \times ( \omega_i \times p_i) + \omega_i \times ( \delta \omega_i \times p_i)
00118    + (1 - \sigma_i) (\dot{\omega}_i \times p_{di} 
00119    + \omega_i \times ( \omega_i \times p_{di}) ) \delta d_i
00120   \f]
00121 
00122   Backward Iterations for \f$i=n, n-1, \ldots, 1\f$. 
00123   Initialize: \f$\delta f_{n+1} = \delta n_{n+1} = 0\f$.
00124 
00125   \f[
00126   \delta \dot{v}_{ci} =
00127   \delta v_i + \delta \omega_i \times r_i 
00128   + \delta \omega_i \times (\omega_i \times r_i) 
00129   + \omega_i \times (\delta \omega_i \times r_i) 
00130   \f]
00131   \f[
00132   \delta F_i = m_i \delta \dot{v}_{ci}
00133   \f]
00134   \f[
00135   \delta N_i = I_{ci} \delta \dot{\omega}_i 
00136   + \delta \omega_i \times (I_{ci} \omega_i) 
00137   + \omega_i \times (I_{ci} \delta \omega_i) 
00138   \f]
00139   \f[
00140   \delta f_i = R_{i+1} [ \delta f_{i+1} ]  
00141   + \delta F_{i} + \sigma_{i+1} Q R_{i+1} [ f_{i+1} ] \delta \theta_{i+1}
00142   \f]
00143   \f[
00144   \delta n_i = R_{i+1} [ \delta n_{i+1} ]  
00145   + \delta N_{i} + p_{i} \times \delta f_{i} 
00146   + r_{i} \times \delta F_{i} + (1 - \sigma_i) (p_{di} \times f_{i}) \delta d_i 
00147   + \sigma_{i+1} Q R_{i+1} [ n_{i+1} ] \delta \theta_{i+1}
00148   \f]
00149   \f[
00150   \delta \tau_i = \sigma_i [ \delta n_i^T (R_i^T z_0) 
00151   - n_i^T (R_i^T Q z_0) \delta \theta_i] + (1 -\sigma_i) [ \delta f_i^T (R_i^T z_0) ]
00152   \f]
00153 */
00154 {
00155    int i;
00156    Matrix Rt, temp;
00157    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00158    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00159    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00160    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00161    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00162    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00163    ltorque = ColumnVector(dof);
00164    dtorque = ColumnVector(dof);
00165    set_q(q);
00166 
00167    vp[0] = gravity;
00168    ColumnVector z0(3);
00169    z0(1) = 0.0; z0(2) = 0.0;  z0(3) = 1.0;
00170    Matrix Q(3,3);
00171    Q = 0.0;
00172    Q(1,2) = -1.0;
00173    Q(2,1) = 1.0;
00174    for(i = 1; i <= dof; i++)
00175    {
00176       Rt = links[i].R.t();
00177       p[i] = ColumnVector(3);
00178       p[i](1) = links[i].get_a();
00179       p[i](2) = links[i].get_d() * Rt(2,3);
00180       p[i](3) = links[i].get_d() * Rt(3,3);
00181       if(links[i].get_joint_type() != 0)
00182       {
00183          dp[i] = ColumnVector(3);
00184          dp[i](1) = 0.0;
00185          dp[i](2) = Rt(2,3);
00186          dp[i](3) = Rt(3,3);
00187       }
00188       if(links[i].get_joint_type() == 0)
00189       {
00190          w[i] = Rt*(w[i-1] + z0*qp(i));
00191          dw[i] = Rt*(dw[i-1] + z0*dqp(i)
00192                      - Q*(w[i-1] + z0*qp(i))*dq(i));
00193          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00194                      + CrossProduct(w[i-1],z0*qp(i)));
00195          dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
00196                       + CrossProduct(dw[i-1],z0*qp(i))
00197                       + CrossProduct(w[i-1],z0*dqp(i))
00198                       - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
00199                       *dq(i));
00200          vp[i] = CrossProduct(wp[i],p[i])
00201                  + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00202                  + Rt*(vp[i-1]);
00203          dvp[i] = CrossProduct(dwp[i],p[i])
00204                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00205                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00206                   + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
00207       }
00208       else
00209       {
00210          w[i] = Rt*w[i-1];
00211          dw[i] = Rt*dw[i-1];
00212          wp[i] = Rt*wp[i-1];
00213          dwp[i] = Rt*dwp[i-1];
00214          vp[i] = Rt*(vp[i-1] + z0*qpp(i)
00215                      + 2.0*CrossProduct(w[i-1],z0*qp(i)))
00216                  + CrossProduct(wp[i],p[i])
00217                  + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00218          dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
00219                       + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
00220                              + CrossProduct(w[i-1],z0*dqp(i))))
00221                   + CrossProduct(dwp[i],p[i])
00222                   + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00223                   + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00224                   + (CrossProduct(wp[i],dp[i])
00225                      + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
00226                   *dq(i);
00227       }
00228       a[i] = CrossProduct(wp[i],links[i].r)
00229              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00230              + vp[i];
00231       da[i] = CrossProduct(dwp[i],links[i].r)
00232               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00233               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00234               + dvp[i];
00235    }
00236 
00237    for(i = dof; i >= 1; i--)
00238    {
00239       F[i] = a[i] * links[i].m;
00240       dF[i] = da[i] * links[i].m;
00241       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00242       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00243               + CrossProduct(w[i],links[i].I*dw[i]);
00244       if(i == dof)
00245       {
00246          f[i] = F[i];
00247          df[i] = dF[i];
00248          n[i] = CrossProduct(p[i],f[i])
00249                 + CrossProduct(links[i].r,F[i]) + N[i];
00250          dn[i] = CrossProduct(p[i],df[i])
00251                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00252          if(links[i].get_joint_type() != 0)
00253             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00254       }
00255       else
00256       {
00257          f[i] = links[i+1].R*f[i+1] + F[i];
00258          df[i] = links[i+1].R*df[i+1] + dF[i];
00259          if(links[i].get_joint_type() == 0)
00260             df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
00261          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00262                 + CrossProduct(links[i].r,F[i]) + N[i];
00263          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
00264                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00265          if(links[i].get_joint_type() == 0)
00266             dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
00267          else
00268             dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00269       }
00270 
00271       if(links[i].get_joint_type() == 0)
00272       {
00273          temp = ((z0.t()*links[i].R)*n[i]);
00274          ltorque(i) = temp(1,1);
00275          temp = ((z0.t()*links[i].R)*dn[i]);
00276          dtorque(i) = temp(1,1);
00277       }
00278       else
00279       {
00280          temp = ((z0.t()*links[i].R)*f[i]);
00281          ltorque(i) = temp(1,1);
00282          temp = ((z0.t()*links[i].R)*df[i]);
00283          dtorque(i) = temp(1,1);
00284       }
00285    }
00286 }
00287 
00288 
00289 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00290                           const ColumnVector & qpp, const ColumnVector & dq,
00291                           const ColumnVector & dqp, const ColumnVector & dqpp,
00292                           ColumnVector & ltorque, ColumnVector & dtorque)
00293 /*!
00294   @brief Delta torque dynamics.
00295 
00296   This function computes
00297   \f[
00298     \delta \tau = D(q) \delta \ddot{q}
00299     + S_1(q,\dot{q}) \delta \dot{q} + S_2(q,\dot{q},\ddot{q}) \delta q    
00300   \f]
00301 
00302   Murray and Neuman Cite_: Murray86 have developed an efficient recursive 
00303   linearized Newton-Euler formulation.   In order to apply the RNE as presented in 
00304   let us define the following variables 
00305 
00306   \f[
00307     p_{di} = \frac{\partial p_i}{\partial d_i} = 
00308     \left [
00309       \begin{array}{ccc}
00310         0 & \sin \alpha_i & \cos \alpha_i 
00311       \end{array}
00312     \right ]^T
00313   \f]
00314   \f[
00315     Q = 
00316     \left [
00317       \begin{array}{ccc}
00318         0 & -1 & 0  \\
00319   1 & 0 & 0  \\
00320   0 & 0 & 0 
00321       \end{array}
00322   \right ]
00323   \f]
00324 
00325   Forward Iterations for \f$i=1, 2, \ldots, n\f$. 
00326   Initialize: \f$\delta \omega_0 = \delta \dot{\omega}_0 = \delta \dot{v}_0 = 0\f$.
00327 
00328   \f[
00329   \delta \omega_i = R_i^T \delta \omega_{i-1} + \sigma_i
00330   (z_0 \delta \dot{\theta}_i - QR_i^T \omega_i \delta \theta_i) 
00331   \f]
00332   \f[
00333   \delta \dot{\omega}_i = R_i^T \delta \dot{w}_{i-1} +
00334   \sigma_i [R_i^T \delta \omega_{i-1} \times z_0 \dot{\theta}_i
00335   + R_i^T \omega_{i-1} \times z_0 \delta \dot{\theta}_i  
00336   + z_0 \ddot{\theta}_i - (QR_i^T \dot{\omega}_{i-1} + QR_i^T \omega_{i-1}
00337   \times \omega{z}_0 \dot{\theta}_i) \delta \theta_i ] 
00338   \f]
00339   \f[
00340   \delta \dot{v}_i = R_i^T\Big(\delta \dot{\omega}_{i-1} \times p_i + \delta \omega_{i-1}
00341   \times(\omega_{i-1} \times p_i) + \omega_{i-1} \times( \delta \omega_{i-1} \times p_i) +
00342   \delta \dot{v}_i\Big) + (1-\sigma_i)\Big(2\delta \omega_i \times z_0\dot{d}_i
00343   + 2\omega_i \times z_0 \delta \dot{d}_i + z_0 \delta \ddot{d}_i\Big) 
00344   - \sigma_i QR_i^T \Big(\dot{\omega}_{i-1} \times p_i + \omega_{i-1} \times(w_{i-1}\times p_i)
00345   + \dot{v}_i\Big) \delta \theta_i + (1-\sigma_i) R_i^T \Big(\dot{\omega}_{i-1} \times
00346   p_{di} + \omega_{i-1} \times(\omega_{i-1}\times p_{di})\Big) \delta d_i
00347   \f]
00348   
00349   Backward Iterations for \f$i=n, n-1, \ldots, 1\f$. 
00350   Initialize: \f$\delta f_{n+1} = \delta n_{n+1} = 0\f$.
00351 
00352   \f[
00353   \delta \dot{v}_{ci} = \delta \dot{v}_i + \delta \dot{\omega}_i\times
00354   r_i + \delta \omega_i \times(\omega_i \times r_i)
00355   + \omega_i \times ( \delta \omega_i \times r_i) 
00356   \f]
00357   \f[
00358   \delta F_i = m_i \delta \dot{v}_{ci}
00359   \f]
00360   \f[
00361   \delta N_i = I_{ci} \delta \dot{\omega}_i + \delta
00362   \omega_i \times (I_{ci}\omega_i) + \omega_i \times
00363   (I_{ci} \delta \omega_i)
00364   \f]
00365   \f[
00366   \delta f_i = R_{i+1} \delta f_{i+1} +
00367   \delta F_i + \sigma_{i+1} R_{i+1} Qf_{i+1} \delta \theta_{i+1}
00368   \f]
00369   \f[
00370   \delta n_i = \delta N_i + R_{i+1} \delta n_{i+1} + r_i\times \delta F_i
00371   + p_{i+1} \times R_{i+1} \delta f_{i+1} 
00372   + \sigma_{i+1} \Big( R_{i+1} Qn_{i+1}
00373   + p_{i+1} \times R_{i+1} Qf_{i+1} \Big) \delta
00374   \theta_{i+1} + (1-\sigma_{i+1} ) p_{di+1} p_{di+1} \times R_{i+1}
00375   f_{i+1} \delta d_{i+1}
00376   \f]
00377   \f[
00378   \delta \tau_i = \sigma \delta n_i^T z_0 +
00379   (1 - \sigma_i) \delta f_i^T z_0
00380   \f]
00381 */
00382 {
00383    int i;
00384    Matrix Rt, temp;
00385    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00386    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00387    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00388    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00389    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00390    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00391    ltorque = ColumnVector(dof);
00392    dtorque = ColumnVector(dof);
00393    set_q(q);
00394 
00395    vp[0] = gravity;
00396    ColumnVector z0(3);
00397    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00398    Matrix Q(3,3);
00399    Q = 0.0;
00400    Q(1,2) = -1.0;
00401    Q(2,1) = 1.0;
00402    for(i = 1; i <= dof; i++)
00403    {
00404       Rt = links[i].R.t();
00405       p[i] = links[i].p;
00406       if(links[i].get_joint_type() != 0)
00407       {
00408          dp[i] = ColumnVector(3);
00409          dp[i](1) = 0.0;
00410          dp[i](2) = Rt(2,3);
00411          dp[i](3) = Rt(3,3);
00412       }
00413       if(links[i].get_joint_type() == 0)
00414       {
00415          w[i] = Rt*w[i-1] + z0*qp(i);
00416          dw[i] = Rt*dw[i-1] + z0*dqp(i)
00417                  - Q*Rt*w[i-1]*dq(i);
00418          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00419                  + z0*qpp(i);
00420          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00421                   + CrossProduct(Rt*w[i-1],z0*dqp(i))
00422                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00423                   + z0*dqpp(i);
00424          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00425                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00426                      + vp[i-1]);
00427          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00428                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00429                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00430                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00431                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00432       }
00433       else
00434       {
00435          w[i] = Rt*w[i-1];
00436          dw[i] = Rt*dw[i-1];
00437          wp[i] = Rt*wp[i-1];
00438          dwp[i] = Rt*dwp[i-1];
00439          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00440                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00441                  + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
00442 
00443          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00444                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00445                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00446                       + (CrossProduct(wp[i-1],dp[i])
00447                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00448                   + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00449                   + z0*dqpp(i);
00450       }
00451       a[i] = CrossProduct(wp[i],links[i].r)
00452              + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00453              + vp[i];
00454       da[i] = CrossProduct(dwp[i],links[i].r)
00455               + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00456               + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00457               + dvp[i];
00458    }
00459 
00460    for(i = dof; i >= 1; i--) {
00461       F[i] = a[i] * links[i].m;
00462       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00463       dF[i] = da[i] * links[i].m;
00464       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00465               + CrossProduct(w[i],links[i].I*dw[i]);
00466 
00467       if(i == dof)
00468       {
00469          f[i] = F[i];
00470          df[i] = dF[i];
00471          n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00472          dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
00473       }
00474       else
00475       {
00476          f[i] = links[i+1].R*f[i+1] + F[i];
00477          df[i] = links[i+1].R*df[i+1] + dF[i];
00478          if(links[i+1].get_joint_type() == 0)
00479             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00480 
00481          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00482                 + CrossProduct(links[i].r,F[i]) + N[i];
00483          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00484                  + CrossProduct(links[i].r,dF[i]) + dN[i];
00485          if(links[i+1].get_joint_type() == 0)
00486             dn[i] += (links[i+1].R*Q*n[i+1] +
00487                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00488          else
00489             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00490       }
00491 
00492       if(links[i].get_joint_type() == 0)
00493       {
00494          temp = z0.t()*n[i];
00495          ltorque(i) = temp(1,1);
00496          temp = z0.t()*dn[i];
00497          dtorque(i) = temp(1,1);
00498       }
00499       else
00500       {
00501          temp = z0.t()*f[i];
00502          ltorque(i) = temp(1,1);
00503          temp = z0.t()*df[i];
00504          dtorque(i) = temp(1,1);
00505       }
00506    }
00507 }
00508 
00509 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00510                                    const ColumnVector & qpp, const ColumnVector & dq,
00511                                    const ColumnVector & dqp, const ColumnVector & dqpp,
00512                                    ColumnVector & ltorque, ColumnVector & dtorque)
00513 /*!
00514   @brief Delta torque dynamics.
00515   
00516   See mRobot::delta_torque for equations.
00517 */
00518 {
00519    int i;
00520    Matrix Rt, temp;
00521    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00522    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00523    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00524    if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00525    if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00526    if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00527    ltorque = ColumnVector(dof);
00528    dtorque = ColumnVector(dof);
00529    set_q(q);
00530 
00531    vp[0] = gravity;
00532    ColumnVector z0(3);
00533    z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00534    Matrix Q(3,3);
00535    Q = 0.0;
00536    Q(1,2) = -1.0;
00537    Q(2,1) = 1.0;
00538    for(i = 1; i <= dof; i++)
00539    {
00540       Rt = links[i].R.t();
00541       p[i] = links[i].p;
00542       if(links[i].get_joint_type() != 0)
00543       {
00544          dp[i] = ColumnVector(3);
00545          dp[i](1) = 0.0;
00546          dp[i](2) = Rt(2,3);
00547          dp[i](3) = Rt(3,3);
00548       }
00549       if(links[i].get_joint_type() == 0)
00550       {
00551          w[i] = Rt*w[i-1] + z0*qp(i);
00552          dw[i] = Rt*dw[i-1] + z0*dqp(i)
00553                  - Q*Rt*w[i-1]*dq(i);
00554          wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00555                  + z0*qpp(i);
00556          dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00557                   + CrossProduct(Rt*w[i-1],z0*dqp(i))
00558                   - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00559                   + z0*dqpp(i);
00560          vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00561                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00562                      + vp[i-1]);
00563          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00564                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00565                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00566                   - Q*Rt*(CrossProduct(wp[i-1],p[i])
00567                           + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00568       }
00569       else
00570       {
00571          w[i] = Rt*w[i-1];
00572          dw[i] = Rt*dw[i-1];
00573          wp[i] = Rt*wp[i-1];
00574          dwp[i] = Rt*dwp[i-1];
00575          vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00576                      + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00577                  + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00578          dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00579                       + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00580                       + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00581                       + (CrossProduct(wp[i-1],dp[i])
00582                          + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00583                   + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00584                   + z0*dqpp(i);
00585       }
00586    }
00587 
00588    for(i = dof; i >= 1; i--) {
00589       F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00590              CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00591       dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
00592               + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
00593               + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
00594       N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
00595              - CrossProduct(vp[i], links[i].mc);
00596       dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00597               + CrossProduct(w[i],links[i].I*dw[i])
00598               - CrossProduct(dvp[i],links[i].mc);
00599 
00600       if(i == dof)
00601       {
00602          f[i] = F[i];
00603          df[i] = dF[i];
00604          n[i] = N[i];
00605          dn[i] = dN[i];
00606       }
00607       else
00608       {
00609          f[i] = links[i+1].R*f[i+1] + F[i];
00610          df[i] = links[i+1].R*df[i+1] + dF[i];
00611          if(links[i+1].get_joint_type() == 0)
00612             df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00613 
00614          n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00615                 + N[i];
00616          dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00617                  + dN[i];
00618          if(links[i+1].get_joint_type() == 0)
00619             dn[i] += (links[i+1].R*Q*n[i+1] +
00620                       CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00621          else
00622             dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00623       }
00624 
00625       if(links[i].get_joint_type() == 0)
00626       {
00627          temp = z0.t()*n[i];
00628          ltorque(i) = temp(1,1);
00629          temp = z0.t()*dn[i];
00630          dtorque(i) = temp(1,1);
00631       }
00632       else
00633       {
00634          temp = z0.t()*f[i];
00635          ltorque(i) = temp(1,1);
00636          temp = z0.t()*df[i];
00637          dtorque(i) = temp(1,1);
00638       }
00639    }
00640 }
00641 
00642 #ifdef use_namespace
00643 }
00644 #endif

ROBOOP v1.21a
Generated Tue Aug 16 16:32:14 2005 by Doxygen 1.4.4