Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
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
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
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
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
00344 link->hhv = link->Ivv * link->sv + link->Iwv.transpose() * link->sw;
00345 link->hhw = link->Iwv * link->sv + link->Iww * link->sw;
00346
00347 link->dd = link->sv.dot(link->hhv) + link->sw.dot(link->hhw) + link->Jm2;
00348
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
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
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
00436
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
00487
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