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(); } } }
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())); } } } }
// 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())); } } } }
/** \note v, dv, dw are not used in the forward dynamics, but are calculated for forward dynamics users. */ void ForwardDynamicsABM::calcABMPhase1(bool updateNonSpatialVariables) { const LinkTraverse& traverse = body->linkTraverse(); const int n = traverse.numLinks(); for(int i=0; i < n; ++i){ DyLink* link = static_cast<DyLink*>(traverse[i]); const DyLink* parent = link->parent(); if(parent){ switch(link->jointType()){ case Link::ROTATIONAL_JOINT: { const Vector3 arm = parent->R() * link->b(); link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a()); link->p().noalias() = arm + parent->p(); link->sw().noalias() = parent->R() * link->a(); link->sv().noalias() = link->p().cross(link->sw()); link->w().noalias() = link->dq() * link->sw() + parent->w(); if(updateNonSpatialVariables){ link->dw().noalias() = parent->dw() + link->dq() * parent->w().cross(link->sw()) + (link->ddq() * link->sw()); link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm); } break; } case Link::SLIDE_JOINT: link->p().noalias() = parent->R() * (link->b() + link->q() * link->d()) + parent->p(); link->R() = parent->R(); link->sw().setZero(); link->sv().noalias() = parent->R() * link->d(); link->w() = parent->w(); if(updateNonSpatialVariables){ link->dw() = parent->dw(); const Vector3 arm = parent->R() * link->b(); link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm) + 2.0 * link->dq() * parent->w().cross(link->sv()) + link->ddq() * link->sv(); } break; case Link::FIXED_JOINT: default: link->p().noalias() = parent->R() * link->b() + parent->p(); link->R() = parent->R(); link->w() = parent->w(); link->vo() = parent->vo(); link->sw().setZero(); link->sv().setZero(); link->cv().setZero(); link->cw().setZero(); if(updateNonSpatialVariables){ link->dw() = parent->dw(); const Vector3 arm = parent->R() * link->b(); link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm); } goto COMMON_CALCS_FOR_ALL_JOINT_TYPES; } // Common for ROTATE and SLIDE link->vo().noalias() = link->dq() * link->sv() + parent->vo(); const Vector3 dsv = parent->w().cross(link->sv()) + parent->vo().cross(link->sw()); const Vector3 dsw = parent->w().cross(link->sw()); link->cv() = link->dq() * dsv; link->cw() = link->dq() * dsw; } COMMON_CALCS_FOR_ALL_JOINT_TYPES: if(updateNonSpatialVariables){ link->v().noalias() = link->vo() + link->w().cross(link->p()); } link->wc().noalias() = link->R() * link->c() + link->p(); // compute I^s (Eq.(6.24) of Kajita's textbook)) const Matrix3 Iw = link->R() * link->I() * link->R().transpose(); const double m = link->m(); const Matrix3 c_hat = hat(link->wc()); link->Iww().noalias() = m * c_hat * c_hat.transpose() + Iw; link->Ivv() << m, 0.0, 0.0, 0.0, m, 0.0, 0.0, 0.0, m; link->Iwv() = m * c_hat; // compute P and L (Eq.(6.25) of Kajita's textbook) const Vector3 P = m * (link->vo() + link->w().cross(link->wc())); const Vector3 L = link->Iww() * link->w() + m * link->wc().cross(link->vo()); link->pf().noalias() = link->w().cross(P); link->ptau().noalias() = link->vo().cross(P) + link->w().cross(L); const Vector3 fg = m * g; const Vector3 tg = link->wc().cross(fg); link->pf() -= fg; link->ptau() -= tg; } }