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(); } } }
/** \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; } }