void forwardKinematics(const MultiBody& mb, MultiBodyConfig& mbc)
{
const std::vector<Joint>& joints = mb.joints();
const std::vector<int>& pred = mb.predecessors();
const std::vector<int>& succ = mb.successors();
const std::vector<sva::PTransformd>& Xt = mb.transforms();
for(std::size_t i = 0; i < joints.size(); ++i)
{
mbc.jointConfig[i] = joints[i].pose(mbc.q[i]);
mbc.parentToSon[i] = mbc.jointConfig[i]*Xt[i];
if(pred[i] != -1)
mbc.bodyPosW[succ[i]] = mbc.parentToSon[i]*mbc.bodyPosW[pred[i]];
else
mbc.bodyPosW[succ[i]] = mbc.parentToSon[i];
}
}
void forwardAcceleration(const MultiBody& mb, MultiBodyConfig& mbc,
const sva::MotionVecd& A_0)
{
const std::vector<Joint>& joints = mb.joints();
const std::vector<int>& pred = mb.predecessors();
const std::vector<int>& succ = mb.successors();
for(std::size_t i = 0; i < joints.size(); ++i)
{
const sva::PTransformd& X_p_i = mbc.parentToSon[i];
const sva::MotionVecd& vj_i = mbc.jointVelocity[i];
sva::MotionVecd ai_tan = joints[i].tanAccel(mbc.alphaD[i]);
const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
if(pred[i] != -1)
mbc.bodyAccB[succ[i]] = X_p_i*mbc.bodyAccB[pred[i]] + ai_tan + vb_i.cross(vj_i);
else
mbc.bodyAccB[succ[i]] = X_p_i*A_0 + ai_tan + vb_i.cross(vj_i);
}
}
void forwardVelocity(const MultiBody& mb, MultiBodyConfig& mbc)
{
const std::vector<Joint>& joints = mb.joints();
const std::vector<int>& pred = mb.predecessors();
const std::vector<int>& succ = mb.successors();
for(std::size_t i = 0; i < joints.size(); ++i)
{
const sva::PTransformd& X_p_i = mbc.parentToSon[i];
mbc.jointVelocity[i] = joints[i].motion(mbc.alpha[i]);
mbc.motionSubspace[i] = joints[i].motionSubspace();
if(pred[i] != -1)
mbc.bodyVelB[succ[i]] = X_p_i*mbc.bodyVelB[pred[i]] + mbc.jointVelocity[i];
else
mbc.bodyVelB[succ[i]] = mbc.jointVelocity[i];
sva::PTransformd E_0_i(mbc.bodyPosW[succ[i]].rotation());
mbc.bodyVelW[succ[i]] = E_0_i.invMul(mbc.bodyVelB[succ[i]]);
}
}
