00001
00002
00003
00004
00005
00006
00007
00008
00014 #include "Body.h"
00015 #include "Link.h"
00016 #include "LinkTraverse.h"
00017 #include "Sensor.h"
00018 #include "ForwardDynamicsCBM.h"
00019
00020 using namespace hrp;
00021 using namespace std;
00022
00023
00024 static const bool CALC_ALL_JOINT_TORQUES = false;
00025 static const bool ROOT_ATT_NORMALIZATION_ENABLED = false;
00026
00027 static const bool debugMode = false;
00028
00029 #include <boost/format.hpp>
00030
00031 template<class TMatrix>
00032 static void putMatrix(TMatrix& M, char* name)
00033 {
00034 if(M.cols() == 1){
00035 std::cout << "Vector " << name << M << std::endl;
00036 } else {
00037 std::cout << "Matrix " << name << ": \n";
00038 for(size_t i=0; i < M.size1(); i++){
00039 for(size_t j=0; j < M.cols(); j++){
00040 std::cout << boost::format(" %6.3f ") % M(i, j);
00041 }
00042 std::cout << std::endl;
00043 }
00044 }
00045 }
00046
00047 template<class TVector>
00048 static void putVector(TVector& M, char* name)
00049 {
00050 std::cout << "Vector " << name << M << std::endl;
00051 }
00052
00053
00054 ForwardDynamicsMM::ForwardDynamicsMM(BodyPtr body) :
00055 ForwardDynamics(body)
00056 {
00057
00058 }
00059
00060
00061 ForwardDynamicsMM::~ForwardDynamicsMM()
00062 {
00063
00064 }
00065
00066
00067 void ForwardDynamicsMM::initialize()
00068 {
00069 Link* root = body->rootLink();
00070 unknown_rootDof = (root->jointType == Link::FREE_JOINT && !root->isHighGainMode ) ? 6 : 0;
00071 given_rootDof = (root->jointType == Link::FREE_JOINT && root->isHighGainMode ) ? 6 : 0;
00072
00073 torqueModeJoints.clear();
00074 highGainModeJoints.clear();
00075
00076 int numLinks = body->numLinks();
00077
00078 for(int i=1; i < numLinks; ++i){
00079 Link* link = body->link(i);
00080 int jointType = link->jointType;
00081 if(jointType == Link::ROTATIONAL_JOINT || jointType == Link::SLIDE_JOINT){
00082 if(link->isHighGainMode){
00083 highGainModeJoints.push_back(link);
00084 } else {
00085 torqueModeJoints.push_back(link);
00086 }
00087 }
00088 }
00089
00090 int n = unknown_rootDof + torqueModeJoints.size();
00091 int m = highGainModeJoints.size();
00092
00093 isNoUnknownAccelMode = (n == 0);
00094
00095 M11.resize(n, n);
00096 M12.resize(n, given_rootDof+m);
00097 b1. resize(n, 1);
00098 c1. resize(n);
00099 d1. resize(n, 1);
00100
00101 qGiven. resize(m);
00102 dqGiven. resize(m);
00103 ddqGiven.resize(given_rootDof+m, 1);
00104
00105 qGivenPrev. resize(m);
00106 dqGivenPrev.resize(m);
00107
00108 ddqorg.resize(numLinks);
00109 uorg. resize(numLinks);
00110
00111 calcPositionAndVelocityFK();
00112
00113 if(!isNoUnknownAccelMode){
00114 calcMassMatrix();
00115 }
00116
00117 ddqGivenCopied = false;
00118
00119 initializeSensors();
00120
00121 if(integrationMode == RUNGEKUTTA_METHOD){
00122
00123 q0. resize(numLinks);
00124 dq0.resize(numLinks);
00125 dq. resize(numLinks);
00126 ddq.resize(numLinks);
00127
00128 preserveHighGainModeJointState();
00129 }
00130 }
00131
00132
00133 void ForwardDynamicsMM::solveUnknownAccels()
00134 {
00135 if(!isNoUnknownAccelMode){
00136 initializeAccelSolver();
00137 solveUnknownAccels(fextTotal, tauextTotal);
00138 }
00139 }
00140
00141
00142 inline void ForwardDynamicsMM::calcAccelFKandForceSensorValues()
00143 {
00144 Vector3 f, tau;
00145 calcAccelFKandForceSensorValues(body->rootLink(), f, tau);
00146 }
00147
00148
00149 void ForwardDynamicsMM::calcNextState()
00150 {
00151 if(isNoUnknownAccelMode && !body->numSensors(Sensor::FORCE)){
00152
00153 calcPositionAndVelocityFK();
00154
00155 } else {
00156
00157 switch(integrationMode){
00158
00159 case EULER_METHOD:
00160 calcMotionWithEulerMethod();
00161 break;
00162
00163 case RUNGEKUTTA_METHOD:
00164 calcMotionWithRungeKuttaMethod();
00165 break;
00166 }
00167
00168 if(ROOT_ATT_NORMALIZATION_ENABLED && unknown_rootDof){
00169 Matrix33& R = body->rootLink()->R;
00170 Matrix33::ColXpr x = R.col(0);
00171 Matrix33::ColXpr y = R.col(1);
00172 Matrix33::ColXpr z = R.col(2);
00173 x.normalize();
00174 z = x.cross(y).normalized();
00175 y = z.cross(x);
00176 }
00177 }
00178
00179 if(sensorsEnabled){
00180 updateSensorsFinal();
00181 }
00182
00183 body->setVirtualJointForces();
00184
00185 ddqGivenCopied = false;
00186 }
00187
00188
00189 void ForwardDynamicsMM::calcMotionWithEulerMethod()
00190 {
00191 sumExternalForces();
00192 solveUnknownAccels();
00193 calcAccelFKandForceSensorValues();
00194
00195 Link* root = body->rootLink();
00196
00197 if(unknown_rootDof){
00198 Vector3 p;
00199 Matrix33 R;
00200 SE3exp(p, R, root->p, root->R, root->w, root->vo, timeStep);
00201 root->p = p;
00202 root->R = R;
00203
00204 root->vo += root->dvo * timeStep;
00205 root->w += root->dw * timeStep;
00206 }
00207
00208 int n = torqueModeJoints.size();
00209 for(int i=0; i < n; ++i){
00210 Link* link = torqueModeJoints[i];
00211 link->q += link->dq * timeStep;
00212 link->dq += link->ddq * timeStep;
00213 }
00214
00215 calcPositionAndVelocityFK();
00216 calcMassMatrix();
00217 }
00218
00219
00220 void ForwardDynamicsMM::calcMotionWithRungeKuttaMethod()
00221 {
00222 int numHighGainJoints = highGainModeJoints.size();
00223 Link* root = body->rootLink();
00224
00225 if(given_rootDof){
00226 pGiven = root->p;
00227 RGiven = root->R;
00228 voGiven = root->vo;
00229 wGiven = root->w;
00230 root->p = pGivenPrev;
00231 root->R = RGivenPrev;
00232 root->vo = voGivenPrev;
00233 root->w = wGivenPrev;
00234 }
00235
00236 for(int i=0; i < numHighGainJoints; ++i){
00237 Link* link = highGainModeJoints[i];
00238 qGiven [i] = link->q;
00239 dqGiven[i] = link->dq;
00240 link->q = qGivenPrev[i];
00241 link->dq = dqGivenPrev[i];
00242 }
00243
00244 if(unknown_rootDof || given_rootDof){
00245 p0 = root->p;
00246 R0 = root->R;
00247 vo0 = root->vo;
00248 w0 = root->w;
00249 }
00250
00251 vo.setZero();
00252 w.setZero();
00253 dvo.setZero();
00254 dw.setZero();
00255
00256 int numLinks = body->numLinks();
00257
00258 for(int i=1; i < numLinks; ++i){
00259 Link* link = body->link(i);
00260 q0 [i] = link->q;
00261 dq0[i] = link->dq;
00262 dq [i] = 0.0;
00263 ddq[i] = 0.0;
00264 }
00265
00266 sumExternalForces();
00267
00268 solveUnknownAccels();
00269 calcAccelFKandForceSensorValues();
00270
00271 integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
00272
00273 calcPositionAndVelocityFK();
00274 calcMassMatrix();
00275 solveUnknownAccels();
00276
00277 integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
00278
00279 calcPositionAndVelocityFK();
00280 calcMassMatrix();
00281 solveUnknownAccels();
00282
00283 integrateRungeKuttaOneStep(2.0 / 6.0, timeStep);
00284
00285 calcPositionAndVelocityFK();
00286 calcMassMatrix();
00287 solveUnknownAccels();
00288
00289 if(unknown_rootDof){
00290 SE3exp(root->p, root->R, p0, R0, w0, vo0, timeStep);
00291 root->vo = vo0 + (dvo + root->dvo / 6.0) * timeStep;
00292 root->w = w0 + (dw + root->dw / 6.0) * timeStep;
00293 }
00294 if(given_rootDof){
00295 root->p = pGiven;
00296 root->R = RGiven;
00297 root->vo = voGiven;
00298 root->w = wGiven;
00299 }
00300
00301 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00302 Link* link = torqueModeJoints[i];
00303 int index = link->index;
00304 link->q = q0 [index] + (dq [index] + link->dq / 6.0) * timeStep;
00305 link->dq = dq0[index] + (ddq[index] + link->ddq / 6.0) * timeStep;
00306 }
00307 for(size_t i=0; i < highGainModeJoints.size(); ++i){
00308 Link* link = highGainModeJoints[i];
00309 link->q = qGiven [i];
00310 link->dq = dqGiven[i];
00311 }
00312
00313 calcPositionAndVelocityFK();
00314 calcMassMatrix();
00315
00316 preserveHighGainModeJointState();
00317 }
00318
00319
00320 void ForwardDynamicsMM::integrateRungeKuttaOneStep(double r, double dt)
00321 {
00322 Link* root = body->rootLink();
00323
00324 if(unknown_rootDof || given_rootDof){
00325 SE3exp(root->p, root->R, p0, R0, root->w, root->vo, dt);
00326 root->vo = vo0 + root->dvo * dt;
00327 root->w = w0 + root->dw * dt;
00328
00329 vo += r * root->vo;
00330 w += r * root->w;
00331 dvo += r * root->dvo;
00332 dw += r * root->dw;
00333 }
00334
00335 int n = body->numLinks();
00336 for(int i=1; i < n; ++i){
00337 Link* link = body->link(i);
00338 link->q = q0 [i] + dt * link->dq;
00339 link->dq = dq0[i] + dt * link->ddq;
00340 dq [i] += r * link->dq;
00341 ddq[i] += r * link->ddq;
00342 }
00343 }
00344
00345
00346 void ForwardDynamicsMM::preserveHighGainModeJointState()
00347 {
00348 if(given_rootDof){
00349 Link* root = body->rootLink();
00350 pGivenPrev = root->p;
00351 RGivenPrev = root->R;
00352 voGivenPrev = root->vo;
00353 wGivenPrev = root->w;
00354 }
00355 for(size_t i=0; i < highGainModeJoints.size(); ++i){
00356 Link* link = highGainModeJoints[i];
00357 qGivenPrev [i] = link->q;
00358 dqGivenPrev[i] = link->dq;
00359 }
00360 }
00361
00362
00363 void ForwardDynamicsMM::calcPositionAndVelocityFK()
00364 {
00365 const LinkTraverse& traverse = body->linkTraverse();
00366 int n = traverse.numLinks();
00367
00368 Link* root = traverse[0];
00369 root_w_x_v = root->w.cross(root->vo + root->w.cross(root->p));
00370
00371 if(given_rootDof){
00372 root->vo = root->v - root->w.cross(root->p);
00373 }
00374
00375 for(int i=0; i < n; ++i){
00376 Link* link = traverse[i];
00377 Link* parent = link->parent;
00378
00379 if(parent){
00380
00381 switch(link->jointType){
00382
00383 case Link::SLIDE_JOINT:
00384 link->p = parent->R * (link->b + link->q * link->d) + parent->p;
00385 link->R = parent->R;
00386 link->sw.setZero();
00387 link->sv.noalias() = parent->R * link->d;
00388 link->w = parent->w;
00389 break;
00390
00391 case Link::ROTATIONAL_JOINT:
00392 link->R.noalias() = parent->R * rodrigues(link->a, link->q);
00393 link->p = parent->R * link->b + parent->p;
00394 link->sw.noalias() = parent->R * link->a;
00395 link->sv = link->p.cross(link->sw);
00396 link->w = link->dq * link->sw + parent->w;
00397 break;
00398
00399 case Link::FIXED_JOINT:
00400 default:
00401 link->p = parent->R * link->b + parent->p;
00402 link->R = parent->R;
00403 link->w = parent->w;
00404 link->vo = parent->vo;
00405 link->sw.setZero();
00406 link->sv.setZero();
00407 link->cv.setZero();
00408 link->cw.setZero();
00409 goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
00410 }
00411
00412 link->vo = link->dq * link->sv + parent->vo;
00413
00414 Vector3 dsv(parent->w.cross(link->sv) + parent->vo.cross(link->sw));
00415 Vector3 dsw(parent->w.cross(link->sw));
00416 link->cv = link->dq * dsv;
00417 link->cw = link->dq * dsw;
00418 }
00419
00420 COMMON_CALCS_FOR_ALL_JOINT_TYPES:
00421
00423 link->v = link->vo + link->w.cross(link->p);
00424
00425 link->wc = link->R * link->c + link->p;
00426 Matrix33 Iw(link->R * link->I * link->R.transpose());
00427 Matrix33 c_hat(hat(link->wc));
00428 link->Iww = link->m * (c_hat * c_hat.transpose()) + Iw;
00429 link->Iwv = link->m * c_hat;
00430
00431 Vector3 P(link->m * (link->vo + link->w.cross(link->wc)));
00432 Vector3 L(link->Iww * link->w + link->m * link->wc.cross(link->vo));
00433
00434 link->pf = link->w.cross(P);
00435 link->ptau = link->vo.cross(P) + link->w.cross(L);
00436
00437 }
00438 }
00439
00440
00446 void ForwardDynamicsMM::calcMassMatrix()
00447 {
00448 Link* root = body->rootLink();
00449 int numLinks = body->numLinks();
00450
00451
00452 for(int i=1; i < numLinks; ++i){
00453 Link* link = body->link(i);
00454 ddqorg[i] = link->ddq;
00455 uorg [i] = link->u;
00456 link->ddq = 0.0;
00457 }
00458
00459
00460 dvoorg = root->dvo;
00461 dworg = root->dw;
00462 root->dvo = g - root_w_x_v;
00463 root->dw.setZero();
00464
00465 setColumnOfMassMatrix(b1, 0);
00466
00467 if(unknown_rootDof){
00468 for(int i=0; i < 3; ++i){
00469 root->dvo[i] += 1.0;
00470 setColumnOfMassMatrix(M11, i);
00471 root->dvo[i] -= 1.0;
00472 }
00473 for(int i=0; i < 3; ++i){
00474 root->dw[i] = 1.0;
00475 Vector3 dw_x_p = root->dw.cross(root->p);
00476 root->dvo -= dw_x_p;
00477 setColumnOfMassMatrix(M11, i + 3);
00478 root->dvo += dw_x_p;
00479 root->dw[i] = 0.0;
00480 }
00481 }
00482 if(given_rootDof){
00483 for(int i=0; i < 3; ++i){
00484 root->dvo[i] += 1.0;
00485 setColumnOfMassMatrix(M12, i);
00486 root->dvo[i] -= 1.0;
00487 }
00488 for(int i=0; i < 3; ++i){
00489 root->dw[i] = 1.0;
00490 Vector3 dw_x_p = root->dw.cross(root->p);
00491 root->dvo -= dw_x_p;
00492 setColumnOfMassMatrix(M12, i + 3);
00493 root->dvo += dw_x_p;
00494 root->dw[i] = 0.0;
00495 }
00496 }
00497
00498 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00499 Link* link = torqueModeJoints[i];
00500 link->ddq = 1.0;
00501 int j = i + unknown_rootDof;
00502 setColumnOfMassMatrix(M11, j);
00503 M11(j, j) += link->Jm2;
00504 link->ddq = 0.0;
00505 }
00506 for(size_t i=0; i < highGainModeJoints.size(); ++i){
00507 Link* link = highGainModeJoints[i];
00508 link->ddq = 1.0;
00509 int j = i + given_rootDof;
00510 setColumnOfMassMatrix(M12, j);
00511 link->ddq = 0.0;
00512 }
00513
00514
00515 for(int i=0; i < M11.cols(); ++i){
00516 M11.col(i) -= b1;
00517 }
00518 for(int i=0; i < M12.cols(); ++i){
00519 M12.col(i) -= b1;
00520 }
00521
00522 for(int i=1; i < numLinks; ++i){
00523 Link* link = body->link(i);
00524 link->ddq = ddqorg[i];
00525 link->u = uorg [i];
00526 }
00527 root->dvo = dvoorg;
00528 root->dw = dworg;
00529
00530 accelSolverInitialized = false;
00531 }
00532
00533
00534 void ForwardDynamicsMM::setColumnOfMassMatrix(dmatrix& M, int column)
00535 {
00536 Vector3 f;
00537 Vector3 tau;
00538 Link* root = body->rootLink();
00539 calcInverseDynamics(root, f, tau);
00540
00541 if(unknown_rootDof){
00542 tau -= root->p.cross(f);
00543 M.block<6,1>(0, column) << f, tau;
00544 }
00545
00546 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00547 M(i + unknown_rootDof, column) = torqueModeJoints[i]->u;
00548 }
00549 }
00550
00551
00552 void ForwardDynamicsMM::calcInverseDynamics(Link* link, Vector3& out_f, Vector3& out_tau)
00553 {
00554 Link* parent = link->parent;
00555 if(parent){
00556 link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
00557 link->dw = parent->dw + link->cw + link->sw * link->ddq;
00558 }
00559
00560 out_f = link->pf;
00561 out_tau = link->ptau;
00562
00563 if(link->child){
00564 Vector3 f_c;
00565 Vector3 tau_c;
00566 calcInverseDynamics(link->child, f_c, tau_c);
00567 out_f += f_c;
00568 out_tau += tau_c;
00569 }
00570
00571 out_f += link->m * link->dvo + link->Iwv.transpose() * link->dw;
00572 out_tau += link->Iwv * link->dvo + link->Iww * link->dw;
00573
00574 link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
00575
00576 if(link->sibling){
00577 Vector3 f_s;
00578 Vector3 tau_s;
00579 calcInverseDynamics(link->sibling, f_s, tau_s);
00580 out_f += f_s;
00581 out_tau += tau_s;
00582 }
00583 }
00584
00585
00586 void ForwardDynamicsMM::sumExternalForces()
00587 {
00588 fextTotal.setZero();
00589 tauextTotal.setZero();
00590
00591 int n = body->numLinks();
00592 for(int i=0; i < n; ++i){
00593 Link* link = body->link(i);
00594 fextTotal += link->fext;
00595 tauextTotal += link->tauext;
00596 }
00597
00598 tauextTotal -= body->rootLink()->p.cross(fextTotal);
00599 }
00600
00601 void ForwardDynamicsMM::calcd1(Link* link, Vector3& out_f, Vector3& out_tau)
00602 {
00603 out_f = -link->fext;
00604 out_tau = -link->tauext;
00605
00606 if(link->child){
00607 Vector3 f_c;
00608 Vector3 tau_c;
00609 calcd1(link->child, f_c, tau_c);
00610 out_f += f_c;
00611 out_tau += tau_c;
00612 }
00613
00614 link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
00615
00616 if(link->sibling){
00617 Vector3 f_s;
00618 Vector3 tau_s;
00619 calcd1(link->sibling, f_s, tau_s);
00620 out_f += f_s;
00621 out_tau += tau_s;
00622 }
00623 }
00624
00625 void ForwardDynamicsMM::initializeAccelSolver()
00626 {
00627 if(!accelSolverInitialized){
00628
00629 if(!ddqGivenCopied){
00630 if(given_rootDof){
00631 Link* root = body->rootLink();
00632 root->dvo = root->dv - root->dw.cross(root->p) - root->w.cross(root->v);
00633 ddqGiven.head(3) = root->dvo;
00634 ddqGiven.segment(3,3) = root->dw;
00635 }
00636 for(size_t i=0; i < highGainModeJoints.size(); ++i){
00637 ddqGiven(given_rootDof+i,0) = highGainModeJoints[i]->ddq;
00638 }
00639 ddqGivenCopied = true;
00640 }
00641
00642 b1 += M12*ddqGiven;
00643
00644 for(unsigned int i=1; i < body->numLinks(); ++i){
00645 Link* link = body->link(i);
00646 uorg [i] = link->u;
00647 }
00648 Vector3 f, tau;
00649 Link* root = body->rootLink();
00650 calcd1(root, f, tau);
00651 for(int i=0; i<unknown_rootDof; i++){
00652 d1(i, 0) = 0;
00653 }
00654 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00655 d1(i + unknown_rootDof, 0) = torqueModeJoints[i]->u;
00656 }
00657 for(unsigned int i=1; i < body->numLinks(); ++i){
00658 Link* link = body->link(i);
00659 link->u = uorg [i];
00660 }
00661
00662 accelSolverInitialized = true;
00663 }
00664 }
00665
00666
00667 void ForwardDynamicsMM::solveUnknownAccels(Link* link, const Vector3& fext, const Vector3& tauext, const Vector3& rootfext, const Vector3& roottauext)
00668 {
00669 Vector3 fextorg = link->fext;
00670 Vector3 tauextorg = link->tauext;
00671 link->fext = fext;
00672 link->tauext = tauext;
00673 for(unsigned int i=1; i < body->numLinks(); ++i){
00674 Link* link = body->link(i);
00675 uorg [i] = link->u;
00676 }
00677 Vector3 f, tau;
00678 Link* root = body->rootLink();
00679 calcd1(root, f, tau);
00680 for(int i=0; i<unknown_rootDof; i++){
00681 d1(i, 0) = 0;
00682 }
00683 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00684 d1(i + unknown_rootDof, 0) = torqueModeJoints[i]->u;
00685 }
00686 for(unsigned int i=1; i < body->numLinks(); ++i){
00687 Link* link = body->link(i);
00688 link->u = uorg [i];
00689 }
00690 link->fext = fextorg;
00691 link->tauext = tauextorg;
00692
00693 solveUnknownAccels(rootfext,roottauext);
00694 }
00695
00696 void ForwardDynamicsMM::solveUnknownAccels(const Vector3& fext, const Vector3& tauext)
00697 {
00698 if(unknown_rootDof){
00699 c1.head(3) = fext;
00700 c1.segment(3,3) = tauext;
00701 }
00702
00703 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00704 c1(i + unknown_rootDof) = torqueModeJoints[i]->u;
00705 }
00706
00707 c1 -= d1;
00708 c1 -= b1.col(0);
00709
00710 dvector a(M11.colPivHouseholderQr().solve(c1));
00711
00712 if(unknown_rootDof){
00713 Link* root = body->rootLink();
00714 root->dw = a.segment(3, 3);
00715 Vector3 dv = a.head(3);
00716 root->dvo = dv - root->dw.cross(root->p) - root_w_x_v;
00717 }
00718
00719 for(size_t i=0; i < torqueModeJoints.size(); ++i){
00720 Link* link = torqueModeJoints[i];
00721 link->ddq = c1(i + unknown_rootDof);
00722 }
00723 }
00724
00725
00726 void ForwardDynamicsMM::calcAccelFKandForceSensorValues(Link* link, Vector3& out_f, Vector3& out_tau)
00727 {
00728 Link* parent = link->parent;
00729
00730 if(parent){
00731 link->dvo = parent->dvo + link->cv + link->sv * link->ddq;
00732 link->dw = parent->dw + link->cw + link->sw * link->ddq;
00733 }
00734
00735 out_f = link->pf;
00736 out_tau = link->ptau;
00737
00738 for(Link* child = link->child; child; child = child->sibling){
00739 Vector3 f, tau;
00740 calcAccelFKandForceSensorValues(child, f, tau);
00741 out_f += f;
00742 out_tau += tau;
00743 }
00744
00745 ForceSensorInfo& info = forceSensorInfo[link->index];
00746
00747 if(CALC_ALL_JOINT_TORQUES || info.hasSensorsAbove){
00748
00749 Vector3 fg(-link->m * g);
00750 Vector3 tg(link->wc.cross(fg));
00751
00752 out_f -= fg;
00753 out_tau -= tg;
00754
00755 out_f -= link->fext;
00756 out_tau -= link->tauext;
00757
00758 out_f += link->m * link->dvo + link->Iwv.transpose() * link->dw;
00759 out_tau += link->Iwv * link->dvo + link->Iww * link->dw;
00760
00761 if(CALC_ALL_JOINT_TORQUES && link->isHighGainMode){
00762 link->u = link->sv.dot(out_f) + link->sw.dot(out_tau);
00763 }
00764
00765 if(info.sensor){
00766 ForceSensor* sensor = info.sensor;
00767 Matrix33 sensorR (link->R * sensor->localR);
00768 Vector3 sensorPos(link->p + link->R * sensor->localPos);
00769 Vector3 f(-out_f);
00770 sensor->f = sensorR.transpose() * f;
00771 sensor->tau = sensorR.transpose() * (-out_tau - sensorPos.cross(f));
00772 }
00773 }
00774 }
00775
00776
00777 void ForwardDynamicsMM::initializeSensors()
00778 {
00779 ForwardDynamics::initializeSensors();
00780
00781 int n = body->numLinks();
00782
00783 forceSensorInfo.resize(n);
00784
00785 for(int i=0; i < n; ++i){
00786 forceSensorInfo[i].sensor = 0;
00787 forceSensorInfo[i].hasSensorsAbove = false;
00788 }
00789
00790 if(sensorsEnabled){
00791
00792 int numForceSensors = body->numSensors(Sensor::FORCE);
00793 for(int i=0; i < numForceSensors; ++i){
00794 ForceSensor* sensor = body->sensor<ForceSensor>(i);
00795 forceSensorInfo[sensor->link->index].sensor = sensor;
00796 }
00797
00798 updateForceSensorInfo(body->rootLink(), false);
00799 }
00800 }
00801
00802
00803 void ForwardDynamicsMM::updateForceSensorInfo(Link* link, bool hasSensorsAbove)
00804 {
00805 ForceSensorInfo& info = forceSensorInfo[link->index];
00806 hasSensorsAbove |= (info.sensor != 0);
00807 info.hasSensorsAbove = hasSensorsAbove;
00808
00809 for(Link* child = link->child; child; child = child->sibling){
00810 updateForceSensorInfo(child, hasSensorsAbove);
00811 }
00812 }