C++ (Cpp) Inertia::massの例

コード例 #1

ファイル: joint-spherical.hpp プロジェクト: zamzami/pinocchio

/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
inline Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const ConstraintRotationalSubspace & )
{
    Eigen::Matrix <double, 6, 3> M;
    //    M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
    M.block <3,3> (Inertia::LINEAR, 0) = alphaSkew ( -Y.mass (),  Y.lever ());
    M.block <3,3> (Inertia::ANGULAR, 0) = (Inertia::Matrix3)(Y.inertia () - Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ()));
    return M;
}

コード例 #2

ファイル: joint-revolute-unaligned.hpp プロジェクト: MaximilienNaveau/pinocchio

    /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
    inline Eigen::Matrix<double,6,1>
    operator*( const Inertia& Y,const ConstraintRevoluteUnaligned & cru)
    { 
      /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
      const double &m                 = Y.mass();
      const Inertia::Vector3 & c      = Y.lever();
      const Inertia::Symmetric3 & I   = Y.inertia();

      Eigen::Matrix<double,6,1> res;
      res.head<3>() = -m*c.cross(cru.axis);
      res.tail<3>() = I*cru.axis + c.cross(res.head<3>());
      return res;
    }

コード例 #3

ファイル: joint-prismatic.hpp プロジェクト: zamzami/pinocchio

 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<2> & )
 { 
   /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
   const double 
   &m = Y.mass(),
   &x = Y.lever()[0],
   &y = Y.lever()[1];
   Eigen::Matrix<double,6,1> res; res << 0.0,0.0,m,
   m*y,
   -m*x,
   0.0;
   return res;
 }

コード例 #4

ファイル: joint-prismatic.hpp プロジェクト: zamzami/pinocchio

 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<1> & )
 { 
   /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
   const double 
   &m = Y.mass(),
   &x = Y.lever()[0],
   &z = Y.lever()[2];
   Eigen::Matrix<double,6,1> res; res << 0.0,m,0.0,
   -m*z,
   0.0,
   m*x ;
   return res;
 }

コード例 #5

ファイル: joint-prismatic.hpp プロジェクト: zamzami/pinocchio

 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 inline Eigen::Matrix<double,6,1>
 operator*( const Inertia& Y,const ConstraintPrismatic<0> & )
 { 
   /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
   const double 
   &m = Y.mass(),
   &y = Y.lever()[1],
   &z = Y.lever()[2];
   Eigen::Matrix<double,6,1> res; res << m,0.0,0.0,
   0.0,
   m*z,
   -m*y ;
   return res;
 }

コード例 #6

ファイル: joint-planar.hpp プロジェクト: florent-lamiraux/pinocchio

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
  {
    Eigen::Matrix <Inertia::Scalar, 6, 3> M;
    const double mass = Y.mass ();
    const Inertia::Vector3 & com = Y.lever ();
    const Symmetric3 & inertia = Y.inertia ();

    M.topLeftCorner <3,3> ().setZero ();
    M.topLeftCorner <2,2> ().diagonal ().fill (mass);

    Inertia::Vector3 mc (mass * com);
    M.rightCols <1> ().head <2> () << mc(1), - mc(0);

    M.bottomLeftCorner <3,2> () << 0., mc(2), - mc(1), 0., mc(1), -mc(0);
    M.rightCols <1> ().tail <3> () = inertia.data ().tail <3> ();
    M.rightCols <1> ().head <2> () = mc.head <2> () * com(2);
    M.rightCols <1> ().tail <1> () = -mc.head <2> ().transpose () * com.head <2> ();

    return M;
  }