# HG changeset patch # User "Gwen Johnson" # Date 1358819159 21600 # Branch angular-kinematics # Node ID 647f8008fa38ad474eed04a86e82641c32f22069 # Parent 99a632435b8c12402068bda49ea0ad7dc7f5a401 fixed bug in spatial inertia matrix calculation diff --git a/include/rbdl/Body.h b/include/rbdl/Body.h --- a/include/rbdl/Body.h +++ b/include/rbdl/Body.h @@ -81,9 +81,9 @@ 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