"Fossies" - the Fresh Open Source Software Archive

Member "heaplayers-351/benchmarks/roboop/source/dynamics.cpp" (17 Oct 2003, 6657 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 "dynamics.cpp" see the Fossies "Dox" file reference documentation.

    1 /*
    2 ROBOOP -- A robotics object oriented package in C++
    3 Copyright (C) 1996, 1997  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: dynamics.cpp,v 1.1 2003/10/17 02:39:04 emery Exp $";
   33 
   34 #include "robot.h"
   35 
   36 
   37 ReturnMatrix Robot::torque(const ColumnVector & q, const ColumnVector & qp,
   38                            const ColumnVector & qpp)
   39 {
   40    int i;
   41    ColumnVector z0(3);
   42    ColumnVector ltorque(dof);
   43    Matrix Rt, temp;
   44    ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
   45    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
   46    if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
   47    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
   48    set_q(q);
   49    w = new ColumnVector[dof+1];
   50    wp = new ColumnVector[dof+1];
   51    vp = new ColumnVector[dof+1];
   52    a = new ColumnVector[dof+1];
   53    f = new ColumnVector[dof+1];
   54    n = new ColumnVector[dof+1];
   55    F = new ColumnVector[dof+1];
   56    N = new ColumnVector[dof+1];
   57    p = new ColumnVector[dof+1];
   58    w[0] = ColumnVector(3);
   59    wp[0] = ColumnVector(3);
   60    vp[0] = gravity;
   61    z0(1) = z0(2) = 0.0;
   62    z0(3) = 1.0;
   63    w[0] = 0.0;
   64    wp[0] = 0.0;
   65    for(i = 1; i <= dof; i++) {
   66       Rt = links[i].R.t();
   67       p[i] = ColumnVector(3);
   68       p[i](1) = links[i].get_a();
   69       p[i](2) = links[i].get_d() * Rt(2,3);
   70       p[i](3) = links[i].get_d() * Rt(3,3);
   71       if(links[i].get_joint_type() == 0) {
   72          w[i] = Rt*(w[i-1] + z0*qp(i));
   73          wp[i] = Rt*(wp[i-1] + z0*qpp(i)
   74                + vec_x_prod(w[i-1],z0*qp(i)));
   75          vp[i] = vec_x_prod(wp[i],p[i])
   76                + vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
   77                + Rt*(vp[i-1]);
   78       } else {
   79          w[i] = Rt*w[i-1];
   80          wp[i] = Rt*wp[i-1];
   81          vp[i] = Rt*(vp[i-1] + z0*qpp(i)
   82                + vec_x_prod(w[i],z0*qp(i)))* 2.0
   83                + vec_x_prod(wp[i],p[i])
   84                + vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
   85       }
   86       a[i] = vec_x_prod(wp[i],links[i].r)
   87             + vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
   88             + vp[i];
   89    }
   90 
   91    for(i = dof; i >= 1; i--) {
   92       F[i] =  a[i] * links[i].m;
   93       N[i] = links[i].I*wp[i] + vec_x_prod(w[i],links[i].I*w[i]);
   94       if(i == dof) {
   95          f[i] = F[i];
   96          n[i] = vec_x_prod(p[i],f[i])
   97                + vec_x_prod(links[i].r,F[i]) + N[i];
   98       } else {
   99          f[i] = links[i+1].R*f[i+1] + F[i];
  100          n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
  101                + vec_x_prod(links[i].r,F[i]) + N[i];
  102       }
  103       if(links[i].get_joint_type() == 0) {
  104          temp = ((z0.t()*links[i].R)*n[i]);
  105          ltorque(i) = temp(1,1);
  106       } else {
  107          temp = ((z0.t()*links[i].R)*f[i]);
  108          ltorque(i) = temp(1,1);
  109       }
  110    }
  111 
  112    delete []p;
  113    delete []N;
  114    delete []F;
  115    delete []n;
  116    delete []f;
  117    delete []a;
  118    delete []vp;
  119    delete []wp;
  120    delete []w;
  121    ltorque.Release(); return ltorque;
  122 }
  123 
  124 ReturnMatrix Robot::torque_novelocity(const ColumnVector & q,
  125                const ColumnVector & qpp)
  126 {
  127    int i;
  128    ColumnVector z0(3);
  129    ColumnVector ltorque(dof);
  130    Matrix Rt, temp;
  131    ColumnVector *wp, *vp, *a, *f, *n, *F, *N, *p;
  132    if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
  133    if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
  134    set_q(q);
  135    wp = new ColumnVector[dof+1];
  136    vp = new ColumnVector[dof+1];
  137    a = new ColumnVector[dof+1];
  138    f = new ColumnVector[dof+1];
  139    n = new ColumnVector[dof+1];
  140    F = new ColumnVector[dof+1];
  141    N = new ColumnVector[dof+1];
  142    p = new ColumnVector[dof+1];
  143    wp[0] = ColumnVector(3);
  144    vp[0] = ColumnVector(3);
  145    z0(1) = z0(2) = 0.0;
  146    z0(3) = 1.0;
  147    vp[0] = 0.0;
  148    wp[0] = 0.0;
  149    for(i = 1; i <= dof; i++) {
  150       Rt = links[i].R.t();
  151       p[i] = ColumnVector(3);
  152       p[i](1) = links[i].get_a();
  153       p[i](2) = links[i].get_d() * Rt(2,3);
  154       p[i](3) = links[i].get_d() * Rt(3,3);
  155       if(links[i].get_joint_type() == 0) {
  156          wp[i] = Rt*(wp[i-1] + z0*qpp(i));
  157          vp[i] = vec_x_prod(wp[i],p[i])
  158                + Rt*(vp[i-1]);
  159       } else {
  160          wp[i] = Rt*wp[i-1];
  161          vp[i] = Rt*(vp[i-1] + z0*qpp(i))
  162                + vec_x_prod(wp[i],p[i]);
  163       }
  164       a[i] = vec_x_prod(wp[i],links[i].r) + vp[i];
  165    }
  166 
  167    for(i = dof; i >= 1; i--) {
  168       F[i] = a[i] * links[i].m;
  169       N[i] = links[i].I*wp[i];
  170       if(i == dof) {
  171          f[i] = F[i];
  172          n[i] = vec_x_prod(p[i],f[i])
  173                + vec_x_prod(links[i].r,F[i]) + N[i];
  174       } else {
  175          f[i] = links[i+1].R*f[i+1] + F[i];
  176          n[i] = links[i+1].R*n[i+1] + vec_x_prod(p[i],f[i])
  177                + vec_x_prod(links[i].r,F[i]) + N[i];
  178       }
  179       if(links[i].get_joint_type() == 0) {
  180          temp = ((z0.t()*links[i].R)*n[i]);
  181          ltorque(i) = temp(1,1);
  182       } else {
  183          temp = ((z0.t()*links[i].R)*f[i]);
  184          ltorque(i) = temp(1,1);
  185       }
  186    }
  187 
  188    delete []p;
  189    delete []N;
  190    delete []F;
  191    delete []n;
  192    delete []f;
  193    delete []a;
  194    delete []vp;
  195    delete []wp;
  196    ltorque.Release(); return ltorque;
  197 }
  198 
  199 ReturnMatrix Robot::inertia(const ColumnVector & q)
  200 {
  201    int i, j;
  202    Matrix M(dof,dof);
  203    ColumnVector torque(dof);
  204    for(i = 1; i <= dof; i++) {
  205       for(j = 1; j <= dof; j++) {
  206          torque(j) = (i == j ? 1.0 : 0.0);
  207       }
  208       torque = torque_novelocity(q,torque);
  209       M.Column(i) = torque;
  210 /*    for(j = 1; j <= dof; j++) {
  211          M(j,i) = torque(j);
  212       }*/
  213    }
  214    M.Release(); return M;
  215 }
  216 
  217 ReturnMatrix Robot::acceleration(const ColumnVector & q,
  218       const ColumnVector & qp, const ColumnVector & tau)
  219 {
  220    ColumnVector qpp(dof);
  221 
  222    qpp = 0.0;
  223    qpp = inertia(q).i()*(tau-torque(q,qp,qpp));
  224    qpp.Release(); return qpp;
  225 }