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) { }