Tree TestDoubleJoint(){
Tree three_link("fake_root_link");
three_link.addSegment(Segment("first_link",Joint("fake_fixed_joint",Joint::None),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"fake_root_link");
three_link.addSegment(Segment("second_link",Joint("first_joint",Joint::RotZ),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"first_link");
three_link.addSegment(Segment("third_link",Joint("second_joint",Joint::RotZ),
Frame::DH(4.0,M_PI_2/2,-3.0,-3.0),
RigidBodyInertia(10,Vector(3,-4,-5),RotationalInertia(0,0.35,0,4,2,0))),"second_link");
return three_link;
}
// construct inertia
RigidBodyInertia toKdl(urdf::InertialPtr i)
{
Frame origin = toKdl(i->origin);
// the mass is frame indipendent
double kdl_mass = i->mass;
// kdl and urdf both specify the com position in the reference frame of the link
Vector kdl_com = origin.p;
// kdl specifies the inertia matrix in the reference frame of the link,
// while the urdf specifies the inertia matrix in the inertia reference frame
RotationalInertia urdf_inertia =
RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
// Rotation operators are not defined for rotational inertia,
// so we use the RigidBodyInertia operators (with com = 0) as a workaround
RigidBodyInertia kdl_inertia_wrt_com_workaround =
origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
// Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
// while the getRotationalInertia method returns the 3d inertia wrt the frame origin
// (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
RotationalInertia kdl_inertia_wrt_com =
kdl_inertia_wrt_com_workaround.getRotationalInertia();
return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
}
Chain Puma560(){
Chain puma560;
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,M_PI_2,0.0,0.0),
RigidBodyInertia(0,Vector::Zero(),RotationalInertia(0,0.35,0,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.4318,0.0,0.0,0.0),
RigidBodyInertia(17.4,Vector(-.3638,.006,.2275),RotationalInertia(0.13,0.524,0.539,0,0,0))));
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0203,-M_PI_2,0.15005,0.0),
RigidBodyInertia(4.8,Vector(-.0203,-.0141,.070),RotationalInertia(0.066,0.086,0.0125,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,M_PI_2,0.4318,0.0),
RigidBodyInertia(0.82,Vector(0,.019,0),RotationalInertia(1.8e-3,1.3e-3,1.8e-3,0,0,0))));
puma560.addSegment(Segment());
puma560.addSegment(Segment());
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,-M_PI_2,0.0,0.0),
RigidBodyInertia(0.34,Vector::Zero(),RotationalInertia(.3e-3,.4e-3,.3e-3,0,0,0))));
puma560.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH(0.0,0.0,0.0,0.0),
RigidBodyInertia(0.09,Vector(0,0,.032),RotationalInertia(.15e-3,0.15e-3,.04e-3,0,0,0))));
puma560.addSegment(Segment());
return puma560;
}
RotationalInertia devech(const Eigen::Matrix<double,6,1> & vec)
{
//In the RotationalInertia initializer, the order is
//Ixx, Iyy, Izz, Ixy, Ixz, Iyz
return RotationalInertia(vec[0],vec[3],vec[5],vec[1],vec[2],vec[4]);
}
Chain KukaLWR_DHnew(){
Chain kukaLWR_DHnew;
//joint 0
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::None),
Frame::DH_Craig1989(0.0, 0.0, 0.31, 0.0)
));
//joint 1
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector::Zero(),
RotationalInertia(0.0,0.0,0.0115343,0.0,0.0,0.0))));
//joint 2
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.4, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,-0.3120511,-0.0038871),
RotationalInertia(-0.5471572,-0.0000302,-0.5423253,0.0,0.0,0.0018828))));
//joint 3
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,-0.0015515,0.0),
RotationalInertia(0.0063507,0.0,0.0107804,0.0,0.0,-0.0005147))));
//joint 4
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.39, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.5216809,0.0),
RotationalInertia(-1.0436952,0.0,-1.0392780,0.0,0.0,0.0005324))));
//joint 5
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, 1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.0119891,0.0),
RotationalInertia(0.0036654,0.0,0.0060429,0.0,0.0,0.0004226))));
//joint 6
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0),
Frame::DH_Craig1989(0.0, -1.5707963, 0.0, 0.0).Inverse()*RigidBodyInertia(2,
Vector(0.0,0.0080787,0.0),
RotationalInertia(0.0010431,0.0,0.0036376,0.0,0.0,0.0000101))));
//joint 7
kukaLWR_DHnew.addSegment(Segment(Joint(Joint::RotZ),
Frame::Identity(),
RigidBodyInertia(2,
Vector::Zero(),
RotationalInertia(0.000001,0.0,0.0001203,0.0,0.0,0.0))));
return kukaLWR_DHnew;
}
static inline RotationalInertia Zero(){
return RotationalInertia(0,0,0,0,0,0);
};