void IDIM::computeY(const MultiBody& mb, const MultiBodyConfig& mbc)
{
const std::vector<Body>& bodies = mb.bodies();
const std::vector<Joint>& joints = mb.joints();
const std::vector<int>& pred = mb.predecessors();
Eigen::Matrix<double, 6, 10> bodyFPhi;
for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
{
const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
Eigen::Matrix<double, 6, 10> vb_i_Phi(IMPhi(vb_i));
bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]);
// bodyFPhi += vb_i x* IMPhi(vb_i)
// is faster to convert each col in a ForceVecd
// than using sva::vector6ToCrossDualMatrix
for(int c = 0; c < 10; ++c)
{
bodyFPhi.col(c).noalias() +=
(vb_i.crossDual(sva::ForceVecd(vb_i_Phi.col(c)))).vector();
}
int iDofPos = mb.jointPosInDof(i);
Y_.block(iDofPos, i*10, joints[i].dof(), 10).noalias() =
mbc.motionSubspace[i].transpose()*bodyFPhi;
int j = i;
while(pred[j] != -1)
{
const sva::PTransformd& X_p_j = mbc.parentToSon[j];
// bodyFPhi = X_p_j^T bodyFPhi
// is faster to convert each col in a ForceVecd
// than using X_p_j.inv().dualMatrix()
for(int c = 0; c < 10; ++c)
{
bodyFPhi.col(c) =
X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector();
}
j = pred[j];
int jDofPos = mb.jointPosInDof(j);
if(joints[j].dof() != 0)
{
Y_.block(jDofPos, i*10, joints[j].dof(), 10).noalias() =
mbc.motionSubspace[j].transpose()*bodyFPhi;
}
}
}
}
