Commits

Gwendolyn Johnson  committed 647f800 Draft

fixed bug in spatial inertia matrix calculation

  • Participants
  • Parent commits 99a6324
  • Branches angular-kinematics

Comments (0)

Files changed (1)

File include/rbdl/Body.h

 			Math::Matrix3d mccT = mcc.transpose();
 
 			mSpatialInertia.set (
-					gr[0] + pa(0, 0), pa(0, 1), pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
-					pa(1, 0), gr[1] + pa(1, 1), pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
-					pa(2, 0), pa(2, 1), gr[2] + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
+					gr[0]*gr[0]*mass + pa(0, 0), pa(0, 1), pa(0, 2), mcc(0, 0), mcc(0, 1), mcc(0, 2),
+					pa(1, 0), gr[1]*gr[1]*mass + pa(1, 1), pa(1, 2), mcc(1, 0), mcc(1, 1), mcc(1, 2),
+					pa(2, 0), pa(2, 1), gr[2]*gr[2]*mass + pa(2, 2), mcc(2, 0), mcc(2, 1), mcc(2, 2),
 					mccT(0, 0), mccT(0, 1), mccT(0, 2), mass, 0., 0.,
 					mccT(1, 0), mccT(1, 1), mccT(1, 2), 0., mass, 0.,
 					mccT(2, 0), mccT(2, 1), mccT(2, 2), 0., 0., mass