"Fossies" - the Fresh Open Source Software Archive

Member "heaplayers-351/benchmarks/roboop/source/comp_dq.cpp" (17 Oct 2003, 6942 Bytes) of package /linux/misc/old/heaplayers_3_5_1.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "comp_dq.cpp" see the Fossies "Dox" file reference documentation.

    1 /*
    2 ROBOOP -- A robotics object oriented package in C++
    3 Copyright (C) 1996  Richard Gourdeau
    4 
    5 This library is free software; you can redistribute it and/or
    6 modify it under the terms of the GNU Library General Public
    7 License as published by the Free Software Foundation; either
    8 version 2 of the License, or (at your option) any later version.
    9 
   10 This library is distributed in the hope that it will be useful,
   11 but WITHOUT ANY WARRANTY; without even the implied warranty of
   12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   13 Library General Public License for more details.
   14 
   15 You should have received a copy of the GNU Library General Public
   16 License along with this library (GNUlgpl.txt); if not, write to the
   17 Free Software Foundation, Inc., 675 Mass Ave, Cambridge,
   18 MA 02139, USA.
   19 
   20 Report problems and direct all questions to:
   21 
   22 Richard Gourdeau
   23 Professeur Agrégé
   24 Departement de mathematiques et de genie industriel
   25 Ecole Polytechnique de Montreal
   26 C.P. 6079, Succ. Centre-Ville
   27 Montreal, Quebec, H3C 3A7
   28 
   29 email: richard.gourdeau@polymtl.ca
   30 */
   31 
   32 static char rcsid[] = "$Id: comp_dq.cpp,v 1.1 2003/10/17 02:39:03 emery Exp $";
   33 
   34 #include "robot.h"
   35 
   36 
   37 void Robot::dq_torque(const ColumnVector & q, const ColumnVector & qp,
   38                      const ColumnVector & qpp, const ColumnVector & dq,
   39                      ColumnVector & ltorque, ColumnVector & dtorque)
   40 {
   41    int i;
   42    ColumnVector z0(3);
   43    Matrix Rt, temp;
   44    Matrix Q(3,3);
   45    ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
   46    ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
   47    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
   48    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
   49    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
   50    ltorque = ColumnVector(dof);
   51    dtorque = ColumnVector(dof);
   52    set_q(q);
   53    w = new ColumnVector[dof+1];
   54    wp = new ColumnVector[dof+1];
   55    vp = new ColumnVector[dof+1];
   56    a = new ColumnVector[dof+1];
   57    f = new ColumnVector[dof+1];
   58    n = new ColumnVector[dof+1];
   59    F = new ColumnVector[dof+1];
   60    N = new ColumnVector[dof+1];
   61    p = new ColumnVector[dof+1];
   62    dw = new ColumnVector[dof+1];
   63    dwp = new ColumnVector[dof+1];
   64    dvp = new ColumnVector[dof+1];
   65    da = new ColumnVector[dof+1];
   66    df = new ColumnVector[dof+1];
   67    dn = new ColumnVector[dof+1];
   68    dF = new ColumnVector[dof+1];
   69    dN = new ColumnVector[dof+1];
   70    dp = new ColumnVector[dof+1];
   71    w[0] = ColumnVector(3);
   72    wp[0] = ColumnVector(3);
   73    vp[0] = gravity;
   74    dw[0] = ColumnVector(3);
   75    dwp[0] = ColumnVector(3);
   76    dvp[0] = ColumnVector(3);
   77    z0 = 0.0;
   78    Q = 0.0;
   79    Q(1,2) = -1.0;
   80    Q(2,1) = 1.0;
   81    z0(3) = 1.0;
   82    w[0] = 0.0;
   83    wp[0] = 0.0;
   84    dw[0] = 0.0;
   85    dwp[0] = 0.0;
   86    dvp[0] = 0.0;
   87    for(i = 1; i <= dof; i++) {
   88       Rt = links[i].R.t();
   89       p[i] = ColumnVector(3);
   90       p[i](1) = links[i].get_a();
   91       p[i](2) = links[i].get_d() * Rt(2,3);
   92       p[i](3) = links[i].get_d() * Rt(3,3);
   93       if(links[i].get_joint_type() != 0) {
   94          dp[i] = ColumnVector(3);
   95          dp[i](1) = 0.0;
   96          dp[i](2) = Rt(2,3);
   97          dp[i](3) = Rt(3,3);
   98       }
   99       if(links[i].get_joint_type() == 0) {
  100          w[i] = Rt*(w[i-1] + z0*qp(i));
  101          dw[i] = Rt*(dw[i-1] - Q*w[i-1]*dq(i));
  102          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
  103                + vec_x_prod(w[i-1],z0*qp(i)));
  104          dwp[i] = Rt*(dwp[i-1]
  105                + vec_x_prod(dw[i-1],z0*qp(i))
  106                - Q*(wp[i-1]+vec_x_prod(w[i-1],z0*qp(i)))
  107                   *dq(i));
  108          vp[i] = vec_x_prod(wp[i],p[i])
  109                + vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
  110                + Rt*(vp[i-1]);
  111          dvp[i] = vec_x_prod(dwp[i],p[i])
  112                + vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
  113                + vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
  114                + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
  115       } else {
  116          w[i] = Rt*w[i-1];
  117          dw[i] = Rt*dw[i-1];
  118          wp[i] = Rt*wp[i-1];
  119          dwp[i] = Rt*dwp[i-1];
  120          vp[i] = Rt*(vp[i-1] + z0*qpp(i)
  121                + vec_x_prod(w[i],z0*qp(i)))* 2.0
  122                + vec_x_prod(wp[i],p[i])
  123                + vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
  124          dvp[i] = Rt*(dvp[i-1]
  125                + (vec_x_prod(dw[i],z0*qp(i))))* 2.0
  126                + vec_x_prod(dwp[i],p[i])
  127                + vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
  128                + vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
  129                + (vec_x_prod(wp[i],dp[i])
  130                + vec_x_prod(w[i],vec_x_prod(w[i],dp[i])))
  131                   *dq(i);
  132       }
  133       a[i] = vec_x_prod(wp[i],links[i].r)
  134             + vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
  135             + vp[i];
  136       da[i] = vec_x_prod(dwp[i],links[i].r)
  137             + vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
  138             + vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
  139             + dvp[i];
  140    }
  141 
  142    for(i = dof; i >= 1; i--) {
  143       F[i] = a[i] * links[i].m;
  144       N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
  145       dF[i] = da[i] * links[i].m;
  146       dN[i] = links[i].I*dwp[i] + vec_x_prod(dw[i],links[i].I*w[i])
  147             + vec_x_prod(w[i],links[i].I*dw[i]);
  148       if(i == dof) {
  149          f[i] = F[i];
  150          n[i] = vec_x_prod(p[i],f[i])
  151                + vec_x_prod(links[i].r,F[i]) + N[i];
  152          df[i] = dF[i];
  153          dn[i] = vec_x_prod(p[i],df[i])
  154                + vec_x_prod(links[i].r,dF[i]) + dN[i];
  155          if(links[i].get_joint_type() != 0) {
  156             dn[i] = dn[i] + vec_x_prod(dp[i],f[i])*dq(i);
  157          }
  158       } else {
  159          f[i] = links[i+1].R*f[i+1] + F[i];
  160          n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
  161                + vec_x_prod(links[i].r,F[i]) + N[i];
  162          if(links[i+1].get_joint_type() == 0) {
  163             df[i] = links[i+1].R*df[i+1] + dF[i] 
  164                   + Q*links[i+1].R*f[i+1]*dq(i+1);
  165          } else {
  166             df[i] = links[i+1].R*df[i+1] + dF[i];
  167          }
  168          dn[i] = links[i+1].R*dn[i+1] + vec_x_prod(p[i],df[i])
  169                + vec_x_prod(links[i].r,dF[i]) + dN[i];
  170          if(links[i+1].get_joint_type() == 0) {
  171             dn[i] = dn[i] + Q*links[i+1].R*n[i+1]*dq(i+1);
  172          }
  173          if(links[i].get_joint_type() != 0) {
  174             dn[i] = dn[i] + vec_x_prod(dp[i],f[i])*dq(i);
  175          }
  176       }
  177       if(links[i].get_joint_type() == 0) {
  178          temp = ((z0.t()*links[i].R)*n[i]);
  179          ltorque(i) = temp(1,1);
  180          temp = ((z0.t()*links[i].R)*dn[i]);
  181          dtorque(i) = temp(1,1);
  182       } else {
  183          temp = ((z0.t()*links[i].R)*f[i]);
  184          ltorque(i) = temp(1,1);
  185          temp = ((z0.t()*links[i].R)*df[i]);
  186          dtorque(i) = temp(1,1);
  187       }
  188    }
  189 
  190    delete []dp;
  191    delete []dN;
  192    delete []dF;
  193    delete []dn;
  194    delete []df;
  195    delete []da;
  196    delete []dvp;
  197    delete []dwp;
  198    delete []dw;
  199    delete []p;
  200    delete []N;
  201    delete []F;
  202    delete []n;
  203    delete []f;
  204    delete []a;
  205    delete []vp;
  206    delete []wp;
  207    delete []w;
  208 }
  209