ForwardDynamicsABM.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00014 #include "ForwardDynamicsABM.h"
00015 #include "Body.h"
00016 #include "Link.h"
00017 #include "LinkTraverse.h"
00018 #include "Sensor.h"
00019 #include <hrpUtil/EigenTypes.h>
00020 
00021 
00022 using namespace hrp;
00023 using namespace std;
00024 
00025 
00026 static const bool debugMode = false;
00027 static const bool rootAttitudeNormalizationEnabled = false;
00028 
00029 
00030 ForwardDynamicsABM::ForwardDynamicsABM(BodyPtr body) :
00031         ForwardDynamics(body),
00032         q0(body->numLinks()),
00033         dq0(body->numLinks()),
00034         dq(body->numLinks()),
00035         ddq(body->numLinks())
00036 {
00037 
00038 }
00039 
00040 
00041 ForwardDynamicsABM::~ForwardDynamicsABM()
00042 {
00043 
00044 }
00045 
00046 
00047 void ForwardDynamicsABM::initialize()
00048 {
00049     initializeSensors();
00050     calcABMFirstHalf();
00051 }
00052 
00053 
00054 inline void ForwardDynamicsABM::calcABMFirstHalf()
00055 {
00056         calcABMPhase1();
00057         calcABMPhase2Part1();
00058 }
00059 
00060 
00061 inline void ForwardDynamicsABM::calcABMLastHalf()
00062 {
00063         calcABMPhase2Part2();
00064         calcABMPhase3();
00065 }
00066 
00067 
00068 void ForwardDynamicsABM::calcNextState()
00069 {
00070         switch(integrationMode){
00071 
00072         case EULER_METHOD:
00073                 calcMotionWithEulerMethod();
00074                 break;
00075                 
00076         case RUNGEKUTTA_METHOD:
00077                 calcMotionWithRungeKuttaMethod();
00078                 break;
00079         }
00080 
00081         if(rootAttitudeNormalizationEnabled){
00082                 Matrix33& R = body->rootLink()->R;
00083                 Matrix33::ColXpr x = R.col(0);
00084                 Matrix33::ColXpr y = R.col(1);
00085                 Matrix33::ColXpr z = R.col(2);
00086                 x.normalize();
00087                 z = x.cross(y).normalized();
00088                 y = z.cross(x);
00089         }
00090 
00091         calcABMFirstHalf();
00092 
00093     if(sensorsEnabled){
00094         updateSensorsFinal();
00095     }
00096 
00097     body->setVirtualJointForces();
00098 }
00099 
00100 
00101 void ForwardDynamicsABM::calcMotionWithEulerMethod()
00102 {
00103     calcABMLastHalf();
00104 
00105     if(sensorsEnabled){
00106         updateForceSensors();
00107     }
00108 
00109     Link* root = body->rootLink();
00110 
00111     if(root->jointType != Link::FIXED_JOINT){
00112         Vector3 p;
00113         Matrix33 R;
00114         SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep);
00115         root->p = p;
00116         root->R = R;
00117 
00118         root->vo += root->dvo * timeStep;
00119         root->w  += root->dw  * timeStep;
00120     }
00121 
00122     int n = body->numLinks();
00123     for(int i=1; i < n; ++i){
00124         Link* link = body->link(i);
00125         link->q  += link->dq  * timeStep;
00126         link->dq += link->ddq * timeStep;
00127     }
00128 }
00129 
00130 
00131 void ForwardDynamicsABM::integrateRungeKuttaOneStep(double r, double dt)
00132 {
00133     Link* root = body->rootLink();
00134 
00135     if(root->jointType != Link::FIXED_JOINT){
00136 
00137         SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt);
00138         root->vo = vo0 + root->dvo * dt;
00139         root->w  = w0  + root->dw  * dt;
00140 
00141         vo  += r * root->vo;
00142         w   += r * root->w;
00143         dvo += r * root->dvo;
00144         dw  += r * root->dw;
00145     }
00146 
00147     int n = body->numLinks();
00148     for(int i=1; i < n; ++i){
00149 
00150         Link* link = body->link(i);
00151 
00152         link->q  =  q0[i] + dt * link->dq;
00153         link->dq = dq0[i] + dt * link->ddq;
00154 
00155         dq[i]  += r * link->dq;
00156         ddq[i] += r * link->ddq;
00157     }
00158 }
00159 
00160 
00161 void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod()
00162 {
00163     Link* root = body->rootLink();
00164 
00165     if(root->jointType != Link::FIXED_JOINT){
00166         p0  = root->p;
00167         R0  = root->R;
00168         vo0 = root->vo;
00169         w0  = root->w;
00170     }
00171 
00172     vo.setZero();
00173     w.setZero();
00174     dvo.setZero();
00175     dw.setZero();
00176 
00177     int n = body->numLinks();
00178     for(int i=1; i < n; ++i){
00179         Link* link = body->link(i);
00180         q0[i]  = link->q;
00181         dq0[i] = link->dq;
00182         dq[i]  = 0.0;
00183         ddq[i] = 0.0;
00184     }
00185 
00186     calcABMLastHalf();
00187 
00188     if(sensorsEnabled){
00189         updateForceSensors();
00190     }
00191 
00192     integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
00193     calcABMPhase1();
00194     calcABMPhase2();
00195     calcABMPhase3();
00196 
00197     integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
00198     calcABMPhase1();
00199     calcABMPhase2();
00200     calcABMPhase3();
00201 
00202     integrateRungeKuttaOneStep(2.0 / 6.0, timeStep);
00203     calcABMPhase1();
00204     calcABMPhase2();
00205     calcABMPhase3();
00206 
00207     if(root->jointType != Link::FIXED_JOINT){
00208         SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep);
00209         root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep;
00210         root->w  = w0  + (dw  + root->dw  / 6.0) * timeStep;
00211     }
00212 
00213     for(int i=1; i < n; ++i){
00214         Link* link = body->link(i);
00215         link->q  =  q0[i] + ( dq[i] + link->dq  / 6.0) * timeStep;
00216         link->dq = dq0[i] + (ddq[i] + link->ddq / 6.0) * timeStep;
00217     }
00218 }
00219 
00220 
00221 void ForwardDynamicsABM::calcABMPhase1()
00222 {
00223     const LinkTraverse& traverse = body->linkTraverse();
00224     int n = traverse.numLinks();
00225 
00226     for(int i=0; i < n; ++i){
00227         Link* link = traverse[i];
00228         Link* parent = link->parent;
00229 
00230         if(parent){
00231             switch(link->jointType){
00232                 
00233             case Link::ROTATIONAL_JOINT:
00234                 link->R.noalias() = parent->R * rodrigues(link->a, link->q);
00235                 link->p = parent->R * link->b + parent->p;
00236                 link->sw.noalias() = parent->R * link->a;
00237                 link->sv = link->p.cross(link->sw);
00238                 link->w = link->dq * link->sw + parent->w;
00239                 break;
00240                 
00241             case Link::SLIDE_JOINT:
00242                 link->p = parent->R * (link->b + link->q * link->d) + parent->p;
00243                 link->R = parent->R;
00244                 link->sw.setZero();
00245                 link->sv.noalias() = parent->R * link->d;
00246                 link->w = parent->w;
00247                 break;
00248                 
00249             case Link::FIXED_JOINT:
00250             default:
00251                 link->p = parent->R * link->b + parent->p;
00252                 link->R = parent->R;
00253                 link->w = parent->w;
00254                 link->vo = parent->vo;
00255                 link->sw.setZero();
00256                 link->sv.setZero();
00257                 link->cv.setZero();
00258                 link->cw.setZero();
00259                 goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
00260             }
00261             
00262             // Common for ROTATE and SLIDE
00263             link->vo = link->dq * link->sv + parent->vo;
00264             Vector3 dsv(parent->w.cross(link->sv) + parent->vo.cross(link->sw));
00265             Vector3 dsw(parent->w.cross(link->sw));
00266             link->cv = link->dq * dsv;
00267             link->cw = link->dq * dsw;
00268         }
00269         
00270         COMMON_CALCS_FOR_ALL_JOINT_TYPES:
00271 
00272         link->v = link->vo + link->w.cross(link->p);
00273         
00274         link->wc = link->R * link->c + link->p;
00275         
00276         // compute I^s (Eq.(6.24) of Kajita's textbook))
00277         Matrix33 Iw(link->R * link->I * link->R.transpose());
00278         
00279         Matrix33 c_hat(hat(link->wc));
00280         link->Iww = link->m * (c_hat * c_hat.transpose()) + Iw;
00281         
00282         link->Ivv <<
00283             link->m, 0.0,     0.0,
00284             0.0,     link->m, 0.0,
00285             0.0,     0.0,     link->m;
00286         
00287         link->Iwv = link->m * c_hat;
00288         
00289         // compute P and L (Eq.(6.25) of Kajita's textbook)
00290         Vector3 P(link->m * (link->vo + link->w.cross(link->wc)));
00291         Vector3 L(link->Iww * link->w + link->m * link->wc.cross(link->vo));
00292         
00293         link->pf = link->w.cross(P);
00294         link->ptau = link->vo.cross(P) + link->w.cross(L);
00295         
00296         Vector3 fg(-link->m * g);
00297         Vector3 tg(link->wc.cross(fg));
00298         
00299         link->pf -= fg;
00300         link->ptau -= tg;
00301     }
00302 }
00303 
00304 
00305 void ForwardDynamicsABM::calcABMPhase2()
00306 {
00307     const LinkTraverse& traverse = body->linkTraverse();
00308     int n = traverse.numLinks();
00309 
00310     for(int i = n-1; i >= 0; --i){
00311         Link* link = traverse[i];
00312 
00313         link->pf   -= link->fext;
00314         link->ptau -= link->tauext;
00315 
00316         // compute articulated inertia (Eq.(6.48) of Kajita's textbook)
00317         for(Link* child = link->child; child; child = child->sibling){
00318 
00319             if(child->jointType == Link::FIXED_JOINT){
00320                 link->Ivv += child->Ivv;
00321                 link->Iwv += child->Iwv;
00322                 link->Iww += child->Iww;
00323 
00324             }else{
00325                 Vector3 hhv_dd(child->hhv / child->dd);
00326                 link->Ivv += child->Ivv - child->hhv * hhv_dd.transpose();
00327                 link->Iwv += child->Iwv - child->hhw * hhv_dd.transpose();
00328                 link->Iww += child->Iww - child->hhw * (child->hhw / child->dd).transpose();
00329             }
00330 
00331             link->pf   += child->Ivv * child->cv + child->Iwv.transpose() * child->cw + child->pf;
00332             link->ptau += child->Iwv * child->cv + child->Iww * child->cw + child->ptau;
00333 
00334             if(child->jointType != Link::FIXED_JOINT){  
00335                 double uu_dd = child->uu / child->dd;
00336                 link->pf   += uu_dd * child->hhv;
00337                 link->ptau += uu_dd * child->hhw;
00338             }
00339         }
00340 
00341         if(i > 0){
00342             if(link->jointType != Link::FIXED_JOINT){
00343                 // hh = Ia * s
00344                 link->hhv = link->Ivv * link->sv + link->Iwv.transpose() * link->sw;
00345                 link->hhw = link->Iwv * link->sv + link->Iww * link->sw;
00346                 // dd = Ia * s * s^T
00347                 link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2;
00348                 // uu = u - hh^T*c + s^T*pp
00349                 link->uu = link->u - (link->hhv.dot(link->cv) + link->hhw.dot(link->cw)
00350                                       + link->sv.dot(link->pf) + link->sw.dot(link->ptau));
00351             }
00352         }
00353     }
00354 }
00355 
00356 
00357 // A part of phase 2 (inbound loop) that can be calculated before external forces are given
00358 void ForwardDynamicsABM::calcABMPhase2Part1()
00359 {
00360     const LinkTraverse& traverse = body->linkTraverse();
00361     int n = traverse.numLinks();
00362 
00363     for(int i = n-1; i >= 0; --i){
00364         Link* link = traverse[i];
00365 
00366         for(Link* child = link->child; child; child = child->sibling){
00367 
00368             if(child->jointType == Link::FIXED_JOINT){
00369                 link->Ivv += child->Ivv;
00370                 link->Iwv += child->Iwv;
00371                 link->Iww += child->Iww;
00372 
00373             }else{
00374                 Vector3 hhv_dd(child->hhv / child->dd);
00375                 link->Ivv += child->Ivv - child->hhv * hhv_dd.transpose();
00376                 link->Iwv += child->Iwv - child->hhw * hhv_dd.transpose();
00377                 link->Iww += child->Iww - child->hhw * (child->hhw / child->dd).transpose();
00378             }
00379 
00380             link->pf   += child->Ivv * child->cv + child->Iwv.transpose() * child->cw;
00381             link->ptau += child->Iwv * child->cv + child->Iww * child->cw;
00382         }
00383 
00384         if(i > 0){
00385             if(link->jointType != Link::FIXED_JOINT){
00386                 link->hhv = link->Ivv * link->sv + link->Iwv.transpose() * link->sw;
00387                 link->hhw = link->Iwv * link->sv + link->Iww * link->sw;
00388                 link->dd  = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2;
00389                 link->uu  = - (link->hhv.dot(link->cv) + link->hhw.dot(link->cw));
00390             }
00391         }
00392     }
00393 }
00394 
00395 
00396 // A remaining part of phase 2 that requires external forces
00397 void ForwardDynamicsABM::calcABMPhase2Part2()
00398 {
00399     const LinkTraverse& traverse = body->linkTraverse();
00400     int n = traverse.numLinks();
00401 
00402     for(int i = n-1; i >= 0; --i){
00403         Link* link = traverse[i];
00404 
00405         link->pf   -= link->fext;
00406         link->ptau -= link->tauext;
00407 
00408         for(Link* child = link->child; child; child = child->sibling){
00409             link->pf   += child->pf;
00410             link->ptau += child->ptau;
00411 
00412             if(child->jointType != Link::FIXED_JOINT){  
00413                 double uu_dd = child->uu / child->dd;
00414                 link->pf   += uu_dd * child->hhv;
00415                 link->ptau += uu_dd * child->hhw;
00416             }
00417         }
00418 
00419         if(i > 0){
00420             if(link->jointType != Link::FIXED_JOINT)
00421                 link->uu += link->u - (link->sv.dot(link->pf) + link->sw.dot(link->ptau));
00422         }
00423     }
00424 }
00425 
00426 
00427 void ForwardDynamicsABM::calcABMPhase3()
00428 {
00429     const LinkTraverse& traverse = body->linkTraverse();
00430 
00431     Link* root = traverse[0];
00432 
00433     if(root->jointType == Link::FREE_JOINT){
00434 
00435         // - | Ivv  trans(Iwv) | * | dvo | = | pf   |
00436         //   | Iwv     Iww     |   | dw  |   | ptau |
00437 
00438         dmatrix Ia(6,6);
00439         Ia << root->Ivv, root->Iwv.transpose(),
00440             root->Iwv, root->Iww;
00441         
00442         dvector p(6);
00443         p << root->pf, root->ptau;
00444         p *= -1.0;
00445         
00446         dvector pm(Ia.colPivHouseholderQr().solve(p));
00447 
00448         root->dvo = pm.head(3);
00449         root->dw  = pm.tail(3);
00450 
00451     } else {
00452         root->dvo.setZero();
00453         root->dw.setZero();
00454     }
00455 
00456     int n = traverse.numLinks();
00457     for(int i=1; i < n; ++i){
00458         Link* link = traverse[i];
00459         Link* parent = link->parent;
00460         if(link->jointType != Link::FIXED_JOINT){
00461             link->ddq = (link->uu - (link->hhv.dot(parent->dvo) + link->hhw.dot(parent->dw))) / link->dd;
00462             link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
00463             link->dw  = parent->dw  + link->cw + link->sw * link->ddq;
00464         }else{
00465             link->ddq = 0.0;
00466             link->dvo = parent->dvo;
00467             link->dw  = parent->dw; 
00468         }
00469     }
00470 }
00471 
00472 
00473 void ForwardDynamicsABM::updateForceSensors()
00474 {
00475     int n = body->numSensors(Sensor::FORCE);
00476     for(int i=0; i < n; ++i){
00477         updateForceSensor(body->sensor<ForceSensor>(i));
00478     }
00479 }
00480 
00481 
00482 void ForwardDynamicsABM::updateForceSensor(ForceSensor* sensor)
00483 {
00484         Link* link = sensor->link;
00485 
00486         //    | f   | = | Ivv  trans(Iwv) | * | dvo | + | pf   |
00487         //    | tau |   | Iwv     Iww     |   | dw  |   | ptau |
00488         
00489         Vector3 f  (-(link->Ivv * link->dvo + link->Iwv.transpose() * link->dw + link->pf));
00490         Vector3 tau(-(link->Iwv * link->dvo + link->Iww * link->dw + link->ptau));
00491 
00492         Matrix33 sensorR(link->R * sensor->localR);
00493         Vector3 fs(sensorR.transpose() * f);
00494         Vector3 sensorPos(link->p + link->R * sensor->localPos);
00495         Vector3 ts(sensorR.transpose() * (tau - sensorPos.cross(f)));
00496 
00497         sensor->f   = fs;
00498         sensor->tau = ts;
00499 
00500         if(debugMode){
00501                 cout << "sensor " << sensor->name << ": ";
00502                 cout << "f = " << f;
00503                 cout << "R = " << sensorR;
00504                 cout << "sensor->f = " << sensor->f;
00505                 cout << endl;
00506         }
00507 }
00508 
00509 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53