示例#1
0
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);
}
示例#2
0
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)
 {
 }