예제 #1
0
 Twist operator/(const Wrench& h, const RigidBodyInertia& I)
 {
     //For mathematical details, check:
     //Featherstone Book (RBDA, 2008) formuma 2.74, page 36 
     double m = I.getMass();
     Vector com_kdl = I.getCOG();
     RotationalInertia Irot = I.getRotationalInertia();
     
     if( m == 0 ) { return Twist::Zero(); }
     
     Eigen::Matrix3d Ic, Ic_inverse, skew_com;
     Eigen::Vector3d com;
     
     com = Eigen::Map<Eigen::Vector3d>(com_kdl.data);
     skew_com = skew(com);
     Ic = Eigen::Map<Eigen::Matrix3d>(Irot.data) + m*skew_com*skew_com;
     Ic_inverse = Ic.inverse();
     
     
     Twist return_value;
     Eigen::Map<Eigen::Vector3d>(return_value.vel.data) =  ((1/m)*Eigen::Matrix3d::Identity() - skew_com*Ic_inverse*skew_com) * Eigen::Map<const Eigen::Vector3d>(h.force.data) + 
                                                           skew_com*Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
                                                           
     Eigen::Map<Eigen::Vector3d>(return_value.rot.data) =  (-Ic_inverse*skew_com)*Eigen::Map<const Eigen::Vector3d>(h.force.data) 
                                                           + Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
 
     return return_value;
 }
예제 #2
0
    Matrix<double,10,1> Vectorize(const RigidBodyInertia & rbd_inertia)
    {
         Matrix<double,10,1> ret;

         Vector MCOG = rbd_inertia.getMass()*rbd_inertia.getCOG();

         //In RigidBodyInertia, the rotational inertia is stored with
         //respect to the link frame of refence, not the COG one

         ret << rbd_inertia.getMass(),
                Map<Vector3d>(MCOG.data),
                vech(rbd_inertia.getRotationalInertia());

         return ret;
    }