void checkMultiBodyEq(const rbd::MultiBody& mb, std::vector<rbd::Body> bodies,
std::vector<rbd::Joint> joints, std::vector<int> pred, std::vector<int> succ,
std::vector<int> parent,
std::vector<sva::PTransformd> Xt)
{
// bodies
BOOST_CHECK_EQUAL_COLLECTIONS(mb.bodies().begin(), mb.bodies().end(),
bodies.begin(), bodies.end());
// joints
BOOST_CHECK_EQUAL_COLLECTIONS(mb.joints().begin(), mb.joints().end(),
joints.begin(), joints.end());
// pred
BOOST_CHECK_EQUAL_COLLECTIONS(mb.predecessors().begin(), mb.predecessors().end(),
pred.begin(), pred.end());
// succ
BOOST_CHECK_EQUAL_COLLECTIONS(mb.successors().begin(), mb.successors().end(),
succ.begin(), succ.end());
// parent
BOOST_CHECK_EQUAL_COLLECTIONS(mb.parents().begin(), mb.parents().end(),
parent.begin(), parent.end());
// Xt
BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(),
mb.transforms().end(),
Xt.begin(), Xt.end());
// nrBodies
BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size());
// nrJoints
BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size());
int params = 0, dof = 0;
for(int i = 0; i < static_cast<int>(joints.size()); ++i)
{
BOOST_CHECK_EQUAL(mb.jointPosInParam(i), params);
BOOST_CHECK_EQUAL(mb.jointsPosInParam()[i], params);
BOOST_CHECK_EQUAL(mb.sJointPosInParam(i), params);
BOOST_CHECK_EQUAL(mb.jointPosInDof(i), dof);
BOOST_CHECK_EQUAL(mb.jointsPosInDof()[i], dof);
BOOST_CHECK_EQUAL(mb.sJointPosInDof(i), dof);
params += joints[i].params();
dof += joints[i].dof();
}
BOOST_CHECK_EQUAL(params, mb.nrParams());
BOOST_CHECK_EQUAL(dof, mb.nrDof());
BOOST_CHECK_THROW(mb.sJointPosInParam(mb.nrJoints()), std::out_of_range);
BOOST_CHECK_THROW(mb.sJointPosInDof(mb.nrJoints()), std::out_of_range);
}
void checkMultiBodyNames(const rbd::MultiBody& mb,
const std::vector<std::string>& bodies,
const std::vector<std::string>& joints)
{
for(const rbd::Body& b: mb.bodies())
{
BOOST_CHECK(std::find(bodies.begin(), bodies.end(), b.name()) != bodies.end());
}
for(const rbd::Joint& j: mb.joints())
{
BOOST_CHECK(std::find(joints.begin(), joints.end(), j.name()) != joints.end());
}
}
