// 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);
}
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;
}
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;
}
RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
//mb=ma
//hb=R*h
//Ib = R(Ia)R' with r=0
Matrix3d R = Map<Matrix3d>(M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
}
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;
}
RigidBodyInertia RigidBodyInertia::RefPoint(const Vector& p){
//mb=ma
//hb=(h-m*r)
//Ib = (Ia+r x h x + (h-m*r) x r x)
Vector hmr = (this->h-this->m*p);
Vector3d r_eig = Map<Vector3d>(p.data);
Vector3d h_eig = Map<Vector3d>(this->h.data);
Vector3d hmr_eig = Map<Vector3d>(hmr.data);
Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = Map<Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;
return RigidBodyInertia(this->m,hmr,Ib,mhi);
}
RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I){
Frame X=T.Inverse();
//mb=ma
//hb=R*(h-m*r)
//Ib = R(Ia+r x h x + (h-m*r) x r x)R'
Vector hmr = (I.h-I.m*X.p);
Vector3d r_eig = Map<Vector3d>(X.p.data);
Vector3d h_eig = Map<Vector3d>(I.h.data);
Vector3d hmr_eig = Map<Vector3d>(hmr.data);
Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
Matrix3d R = Map<Matrix3d>(X.M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R*((Map<Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
}
RigidBodyInertia deVectorize(const Eigen::Matrix<double,10,1> & vec)
{
Vector COG;
if( vec[0] != 0 ) {
COG = Vector(vec[1],vec[2],vec[3]);
COG = COG/vec[0];
} else {
COG = Vector(0.0,0.0,0.0);
}
Vector3d vCOG = Map<Vector3d>(COG.data);
//In the constructor, the request rotational inertia is the one w.r.t. the COG
RotationalInertia I_o = devech(vec.tail<6>());
RotationalInertia I_c;
Map<Matrix3d>(I_c.data) = Map<Matrix3d>(I_o.data)+
vec[0]*(vCOG*vCOG.transpose()-vCOG.dot(vCOG)*Matrix3d::Identity());
return RigidBodyInertia(vec[0],COG,I_c);
}
/**
* Creates an inertia with zero mass, and zero RotationalInertia
*/
static inline RigidBodyInertia Zero(){
return RigidBodyInertia(0.0,Vector::Zero(),RotationalInertia::Zero());
};
RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
}
RigidBodyInertia operator*(double a,const RigidBodyInertia& I){
return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
}
ArticulatedBodyInertia::ArticulatedBodyInertia(double m, const Vector& c, const RotationalInertia& Ic)
{
*this = RigidBodyInertia(m,c,Ic);
}
