// A remaining part of phase 2 that requires external forces void ForwardDynamicsABM::calcABMPhase2Part2() { const LinkTraverse& traverse = body->linkTraverse(); const int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ DyLink* link = static_cast<DyLink*>(traverse[i]); link->pf() -= link->f_ext(); link->ptau() -= link->tau_ext(); for(DyLink* child = link->child(); child; child = child->sibling()){ link->pf() += child->pf(); link->ptau() += child->ptau(); if(!child->isFixedJoint()){ const double uu_dd = child->uu() / child->dd(); link->pf() += uu_dd * child->hhv(); link->ptau() += uu_dd * child->hhw(); } } if(i > 0){ if(!link->isFixedJoint()){ link->uu() += link->u() - (link->sv().dot(link->pf()) + link->sw().dot(link->ptau())); } } } }
void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod() { DyLink* root = body->rootLink(); if(!root->isFixedJoint()){ T0 = root->T(); vo0 = root->vo(); w0 = root->w(); } vo.setZero(); w.setZero(); dvo.setZero(); dw.setZero(); int n = body->numLinks(); for(int i=1; i < n; ++i){ DyLink* link = body->link(i); q0[i] = link->q(); dq0[i] = link->dq(); dq[i] = 0.0; ddq[i] = 0.0; } calcABMLastHalf(); if(!sensorHelper.forceSensors().empty()){ updateForceSensors(); } integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); integrateRungeKuttaOneStep(2.0 / 6.0, timeStep); calcABMPhase1(); calcABMPhase2(); calcABMPhase3(); if(!root->isFixedJoint()){ SE3exp(root->T(), T0, w0, vo0, timeStep); root->vo() = vo0 + (dvo + root->dvo() / 6.0) * timeStep; root->w() = w0 + (dw + root->dw() / 6.0) * timeStep; } for(int i=1; i < n; ++i){ DyLink* link = body->link(i); link->q() = q0[i] + ( dq[i] + link->dq() / 6.0) * timeStep; link->dq() = dq0[i] + (ddq[i] + link->ddq() / 6.0) * timeStep; } }
void ForwardDynamicsABM::calcABMPhase2() { const LinkTraverse& traverse = body->linkTraverse(); const int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ DyLink* link = static_cast<DyLink*>(traverse[i]); link->pf() -= link->f_ext(); link->ptau() -= link->tau_ext(); // compute articulated inertia (Eq.(6.48) of Kajita's textbook) for(DyLink* child = link->child(); child; child = child->sibling()){ if(child->isFixedJoint()){ link->Ivv() += child->Ivv(); link->Iwv() += child->Iwv(); link->Iww() += child->Iww(); }else{ const Vector3 hhv_dd = child->hhv() / child->dd(); link->Ivv().noalias() += child->Ivv() - child->hhv() * hhv_dd.transpose(); link->Iwv().noalias() += child->Iwv() - child->hhw() * hhv_dd.transpose(); link->Iww().noalias() += child->Iww() - child->hhw() * (child->hhw() / child->dd()).transpose(); } link->pf() .noalias() += child->Ivv() * child->cv() + child->Iwv().transpose() * child->cw() + child->pf(); link->ptau().noalias() += child->Iwv() * child->cv() + child->Iww() * child->cw() + child->ptau(); if(!child->isFixedJoint()){ const double uu_dd = child->uu() / child->dd(); link->pf() += uu_dd * child->hhv(); link->ptau() += uu_dd * child->hhw(); } } if(i > 0){ if(!link->isFixedJoint()){ // hh = Ia * s link->hhv().noalias() = link->Ivv() * link->sv() + link->Iwv().transpose() * link->sw(); link->hhw().noalias() = link->Iwv() * link->sv() + link->Iww() * link->sw(); // dd = Ia * s * s^T link->dd() = link->sv().dot(link->hhv()) + link->sw().dot(link->hhw()) + link->Jm2(); // uu = u - hh^T*c + s^T*pp link->uu() = link->u() - (link->hhv().dot(link->cv()) + link->hhw().dot(link->cw()) + link->sv().dot(link->pf()) + link->sw().dot(link->ptau())); } } } }
void ForwardDynamicsABM::calcMotionWithEulerMethod() { calcABMLastHalf(); if(!sensorHelper.forceSensors().empty()){ updateForceSensors(); } DyLink* root = body->rootLink(); if(!root->isFixedJoint()){ root->dv() = root->dvo() - root->p().cross(root->dw()) + root->w().cross(root->vo() + root->w().cross(root->p())); Position T; SE3exp(T, root->T(), root->w(), root->vo(), timeStep); root->T() = T; root->vo() += root->dvo() * timeStep; root->w() += root->dw() * timeStep; } int n = body->numLinks(); for(int i=1; i < n; ++i){ DyLink* link = body->link(i); link->q() += link->dq() * timeStep; link->dq() += link->ddq() * timeStep; } }
void ForwardDynamicsABM::integrateRungeKuttaOneStep(double r, double dt) { DyLink* root = body->rootLink(); if(!root->isFixedJoint()){ SE3exp(root->T(), T0, root->w(), root->vo(), dt); root->vo().noalias() = vo0 + root->dvo() * dt; root->w().noalias() = w0 + root->dw() * dt; vo += r * root->vo(); w += r * root->w(); dvo += r * root->dvo(); dw += r * root->dw(); } int n = body->numLinks(); for(int i=1; i < n; ++i){ DyLink* link = body->link(i); link->q() = q0[i] + dt * link->dq(); link->dq() = dq0[i] + dt * link->ddq(); dq[i] += r * link->dq(); ddq[i] += r * link->ddq(); } }
// A part of phase 2 (inbound loop) that can be calculated before external forces are given void ForwardDynamicsABM::calcABMPhase2Part1() { const LinkTraverse& traverse = body->linkTraverse(); const int n = traverse.numLinks(); for(int i = n-1; i >= 0; --i){ DyLink* link = static_cast<DyLink*>(traverse[i]); for(DyLink* child = link->child(); child; child = child->sibling()){ if(child->isFixedJoint()){ link->Ivv() += child->Ivv(); link->Iwv() += child->Iwv(); link->Iww() += child->Iww(); }else{ const Vector3 hhv_dd = child->hhv() / child->dd(); link->Ivv().noalias() += child->Ivv() - child->hhv() * hhv_dd.transpose(); link->Iwv().noalias() += child->Iwv() - child->hhw() * hhv_dd.transpose(); link->Iww().noalias() += child->Iww() - child->hhw() * (child->hhw() / child->dd()).transpose(); } link->pf() .noalias() += child->Ivv() * child->cv() + child->Iwv().transpose() * child->cw(); link->ptau().noalias() += child->Iwv() * child->cv() + child->Iww() * child->cw(); } if(i > 0){ if(!link->isFixedJoint()){ link->hhv().noalias() = link->Ivv() * link->sv() + link->Iwv().transpose() * link->sw(); link->hhw().noalias() = link->Iwv() * link->sv() + link->Iww() * link->sw(); link->dd() = link->sv().dot(link->hhv()) + link->sw().dot(link->hhw()) + link->Jm2(); link->uu() = -(link->hhv().dot(link->cv()) + link->hhw().dot(link->cw())); } } } }
void ForwardDynamicsABM::calcABMPhase3() { const LinkTraverse& traverse = body->linkTraverse(); DyLink* root = static_cast<DyLink*>(traverse[0]); if(root->isFreeJoint()){ // - | Ivv trans(Iwv) | * | dvo | = | pf | // | Iwv Iww | | dw | | ptau | Eigen::Matrix<double, 6, 6> M; M << root->Ivv(), root->Iwv().transpose(), root->Iwv(), root->Iww(); Eigen::Matrix<double, 6, 1> f; f << root->pf(), root->ptau(); f *= -1.0; Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f)); root->dvo() = a.head<3>(); root->dw() = a.tail<3>(); } else { root->dvo().setZero(); root->dw().setZero(); } const int n = traverse.numLinks(); for(int i=1; i < n; ++i){ DyLink* link = static_cast<DyLink*>(traverse[i]); const DyLink* parent = link->parent(); if(!link->isFixedJoint()){ link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd(); link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq(); link->dw().noalias() = parent->dw() + link->cw() + link->sw() * link->ddq(); }else{ link->ddq() = 0.0; link->dvo() = parent->dvo(); link->dw() = parent->dw(); } } }