sva::RBInertiad MultiBodyGraph::mergeInertia(const sva::RBInertiad& parentInertia,
const sva::RBInertiad& childInertia, const Joint& joint,
const sva::PTransformd& X_p_j,
const std::map<int, std::vector<double>>& jointPosById)
{
if(jointPosById.find(joint.id()) == jointPosById.end())
{
std::ostringstream msg;
msg << "jointPosById must contain joint id " <<
joint.id() << " configuration";
throw std::out_of_range(msg.str());
}
if(int(jointPosById.at(joint.id()).size()) != joint.params())
{
std::ostringstream msg;
msg << "joint id " << joint.id() << " need " << joint.params() <<
" parameters";
throw std::domain_error(msg.str());
}
sva::PTransformd jointConfig = joint.pose(jointPosById.at(joint.id()));
// transformation from current body to joint in next body
sva::PTransformd X_cb_jnb = jointConfig*X_p_j;
// set merged sub inertia in current body base and add it to the current inertia
return parentInertia + X_cb_jnb.transMul(childInertia);
}
void MultiBodyGraph::addJoint(const Joint& J)
{
if(J.id() < 0)
{
std::ostringstream msg;
msg << "Joint id must be greater than zero";
throw std::domain_error(msg.str());
}
// check that the joint id don't exist
if(jointId2Joint_.find(J.id()) != jointId2Joint_.end())
{
std::ostringstream msg;
msg << "Joint id: " << J.id() << " already exist.";
throw std::domain_error(msg.str());
}
// check that the joint name don't exist
if(jointName2Id_.find(J.name()) != jointName2Id_.end())
{
std::ostringstream msg;
msg << "Joint name: " << J.name() << " already exist.";
throw std::domain_error(msg.str());
}
joints_.push_back(std::make_shared<Joint>(J));
jointId2Joint_[J.id()] = joints_.back();
jointName2Id_[J.name()] = J.id();
}