Applied fix for computation of link center-of-mass when loading an SD…

…F file.
YinjieLin · Mar 3, 2016 · e14f832 · e14f832
1 parent 24a17a5
commit e14f832
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/drake/systems/plants/RigidBodyTreeSDF.cpp b/drake/systems/plants/RigidBodyTreeSDF.cpp
@@ -34,7 +34,7 @@ void parseSDFInertial(shared_ptr<RigidBody> body, XMLElement* node,
 
   parseScalarValue(node, "mass", body->mass);
 
-  body->com << T(0, 3), T(1, 3), T(2, 3);
+  body->com = T_link.inverse() * T.translation();
 
   Matrix<double, TWIST_SIZE, TWIST_SIZE> I =
       Matrix<double, TWIST_SIZE, TWIST_SIZE>::Zero();