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

ROBOOP v1.21a
Generated Tue Nov 23 16:35:51 2004 by Doxygen 1.3.9.1