Ejemplo n.º 1
0
    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;
    }
Ejemplo n.º 2
0
// 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;
 }
Ejemplo n.º 4
0
 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;
 }    
Ejemplo n.º 6
0
 static inline RotationalInertia Zero(){
     return RotationalInertia(0,0,0,0,0,0);
 };