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]; } }