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()); } }