Gwendolyn Johnson avatar Gwendolyn Johnson committed 647f800 Draft

fixed bug in spatial inertia matrix calculation

Comments (0)

Files changed (1)

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
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.