Spatial tensor bottom left block is missing a negative sign

[balakumar-s]

If you're not reporting a bug, please use the forum to ask questions.

Bug Report

Please answer the following questions for yourself before reporting a bug.

• [Y] I checked the documentation and found no answer.
• [Y] I checked to make sure that this issue has not already been filed.

Environment

• DART version: master

Expected Behavior

The computeSpatialTensor() is missing a negative sign here

This makes verifySpatialTensor() fail. Is there some reason for not having the negative sign?

Shouldn't the line be:
mSpatialTensor.block<3,3>(3,0) = -mMass*C.transpose();

[jslee02]

computeSpatialTensor() seems correct to me. One simple sanity check is that the spatial tensor should be symmetric. This means the transpose of top right block matric should be equal to the bottom left block.

Note that negating a skewsymmetric matrix is identical to transpose it so -mMass*C.transpose() == mMass*C, which leads to the tensor assymetric.

I'm curious how it makes verifySpatialTensor() fail. Could you elaborate a bit how you created Inertia for checking computeSpatialTensor() and which warning message you're seeing?

[balakumar-s]

Here is the function that I am running:

SkeletonPtr createCylinder(const Eigen::Isometry3d& _T,
                             double _radius,
                             double _height,
                             double _mass)
{
  dart::dynamics::SkeletonPtr newSkeleton = dart::dynamics::Skeleton::create("object");

  dart::dynamics::ShapePtr newShape(new dart::dynamics::CylinderShape(_radius, _height));

  dart::dynamics::BodyNode::Properties bodyProp;
  bodyProp.mName = "cylinder_link";
  bodyProp.mInertia.setMass(_mass);
  const Eigen::Vector3d com(0.01,0.03,0.05);


  // Set up inertia for the domino
  

  double i_xx=(1.0/12.0)*_mass*(3.0*_radius*_radius+_height*_height);
  double i_yy=(1.0/12.0)*_mass*(3.0*_radius*_radius+_height*_height);
  double i_zz=0.5*_mass*_radius*_radius;
  double i_xy=0.0;
  double i_xz=0.0;
  double i_yz=0.0;
  //cerr<<com<<endl;
  dart::dynamics::Inertia inertia(_mass,com[0],com[1],com[2],i_xx,i_yy,i_zz,i_xy,i_xz,i_yz);

  //std::cerr<<"valid moment: "<<bool(inertia.verifyMoment(inertia.getMoment()))<<std::endl;

  //std::cerr<<inertia.getMoment()<<std::endl;
  
  std::cerr<<"valid spatial tensor: "<<bool(inertia.verifySpatialTensor(inertia.getSpatialTensor())<<std::endl;
  std::cerr<<inertia.getSpatialTensor()<<std::endl;
  bodyProp.mInertia=inertia;

  dart::dynamics::FreeJoint::Properties jointProp;
  jointProp.mName = "cylinder_joint";
  jointProp.mT_ParentBodyToJoint = _T;

  
  auto pair = newSkeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr, jointProp, bodyProp);
  
  auto shapeNode = pair.second->createShapeNodeWith<
    dart::dynamics::VisualAspect,
    dart::dynamics::CollisionAspect,
    dart::dynamics::DynamicsAspect>(newShape);


  return newSkeleton;
}

Here is the warning:
Warning [Inertia.cpp:366] [Inertia::verifySpatialTensor] Values for entries (0,4) and (3,1) differ by -0.0644609 which is more than the permitted tolerance (1e-08). The bottom-left block should be the transpose of the top-right block.
Warning [Inertia.cpp:366] [Inertia::verifySpatialTensor] Values for entries (0,5) and (3,2) differ by 0.0395585 which is more than the permitted tolerance (1e-08). The bottom-left block should be the transpose of the top-right block.
Warning [Inertia.cpp:366] [Inertia::verifySpatialTensor] Values for entries (1,5) and (4,2) differ by -0.0110245 which is more than the permitted tolerance (1e-08). The bottom-left block should be the transpose of the top-right block.

And here is the output spatial tensor:

0.0687108 -0.0003891 -0.0006485 0 -0.06485 0.03891
-0.0003891 0.0676732 -0.0019455 0.06485 0 -0.01297
-0.0006485 -0.0019455 0.105057 -0.03891 0.01297 0
0 0.06485 -0.03891 1.297 0 0
-0.06485 0 0.01297 0 1.297 0
0.03891 -0.01297 0 0 0 1.297

[jslee02]

Thank you for sharing the code. The spatial tensor looks correct, but not the verification code. Let me fix it.

[balakumar-s]

Thanks for sorting this out.