BodyNode::getWorldCOM() returns now vector from the world origin to b…

…ody node origin in the world frame.
dartsim · Dec 13, 2013 · dc3bdeb · dc3bdeb
1 parent e399f38
commit dc3bdeb
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 7 deletions.
diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp
@@ -470,7 +470,7 @@ const Eigen::Vector3d& BodyNode::getLocalCOM() const {
 }
 
 Eigen::Vector3d BodyNode::getWorldCOM() const {
-  return mW.linear() * mCenterOfMass;
+  return mW * mCenterOfMass;
 }
 
 Eigen::Matrix6d BodyNode::getInertia() const {

diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h
@@ -155,7 +155,7 @@ class BodyNode {
   ///        mass in the body frame.
   const Eigen::Vector3d& getLocalCOM() const;
 
-  /// \brief Get a point vector from the origin of body frame to the center of
+  /// \brief Get a point vector from the origin world frame to the center of
   ///        mass in the world frame.
   Eigen::Vector3d getWorldCOM() const;
 

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -636,17 +636,15 @@ void Skeleton::setConstraintForceVector(const Eigen::VectorXd& _Fc) {
 }
 
 Eigen::Vector3d Skeleton::getWorldCOM() {
-  Eigen::Vector3d com(0, 0, 0);
+  Eigen::Vector3d com(0.0, 0.0, 0.0);
 
-  assert(mTotalMass != 0);
   const int nNodes = getNumBodyNodes();
-
   for (int i = 0; i < nNodes; i++) {
     BodyNode* bodyNode = getBodyNode(i);
-    com += bodyNode->getMass() *
-           (bodyNode->getWorldTransform() * bodyNode->getLocalCOM());
+    com += bodyNode->getMass() * bodyNode->getWorldCOM();
   }
 
+  assert(mTotalMass != 0.0);
   return com / mTotalMass;
 }