ForwardDynamicsCBM.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 "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         // preserve and clear the joint accelerations
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         // preserve and clear the root link acceleration
00460         dvoorg = root->dvo;
00461         dworg  = root->dw;
00462         root->dvo = g - root_w_x_v;   // dv = g, dw = 0
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; // motor inertia
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         // subtract the constant term
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 //for ConstraintForceSolver 
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 }


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