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 KinFamTest::JointTest()
{
double q;
Joint j;
j=Joint("Joint 1", Joint::None);
CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
random(q);
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
random(q);
j=Joint("Joint 2", Joint::RotX);
CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
random(q);
j=Joint("Joint 3", Joint::RotY);
CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
random(q);
j=Joint("Joint 4", Joint::RotZ);
CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
random(q);
j=Joint("Joint 5", Joint::TransX);
CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
random(q);
j=Joint("Joint 6", Joint::TransY);
CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
random(q);
j=Joint("Joint 7", Joint::TransZ);
CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));
}
Segment::Segment(const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
name("NoName"),
joint(_joint),I(_I),
f_tip(_joint.pose(0).Inverse() * _f_tip)
{
}
Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
name(_name),
joint(_joint),I(_I),
f_tip(_joint.pose(0).Inverse() * _f_tip)
{
}