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