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 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()));
}
}
}
}
// 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::initialize()
{
DyLink* rootLink = body->rootLink();
rootLink->sw().setZero();
rootLink->sv().setZero();
rootLink->cv().setZero();
rootLink->cw().setZero();
rootLink->hhv().setZero();
rootLink->hhw().setZero();
rootLink->uu() = 0.0;
rootLink->dd() = 0.0;
initializeSensors();
calcABMFirstHalf();
}
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();
}
}
}