dartsim · jslee02 · Jul 22, 2016 · Jul 19, 2016 · Jul 21, 2016 · Jul 21, 2016
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,6 +2,10 @@
 
 ### DART 6.1.0 (2016-XX-XX)
 
+* Dynamics
+
+  * Added `computeLagrangian()` to `MetaSkeleton` and `BodyNode`: [#746](https://github.com/dartsim/dart/pull/746)
+
 * Misc improvements and bug fixes
 
   * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)

diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp
@@ -1901,18 +1901,37 @@ const Eigen::Vector6d& BodyNode::getConstraintImpulse() const
   return mConstraintImpulse;
 }
 
+//==============================================================================
+double BodyNode::computeLagrangian(const Eigen::Vector3d& gravity) const
+{
+  return computeKineticEnergy() - computePotentialEnergy(gravity);
+}
+
 //==============================================================================
 double BodyNode::getKineticEnergy() const
+{
+  return computeKineticEnergy();
+}
+
+//==============================================================================
+double BodyNode::computeKineticEnergy() const
 {
   const Eigen::Vector6d& V = getSpatialVelocity();
-  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
-  return 0.5 * V.dot(mI * V);
+  const Eigen::Matrix6d& G = mAspectProperties.mInertia.getSpatialTensor();
+
+  return 0.5 * V.dot(G * V);
 }
 
 //==============================================================================
 double BodyNode::getPotentialEnergy(const Eigen::Vector3d& _gravity) const
 {
-  return -getMass() * getWorldTransform().translation().dot(_gravity);
+  return computePotentialEnergy(_gravity);
+}
+
+//==============================================================================
+double BodyNode::computePotentialEnergy(const Eigen::Vector3d& gravity) const
+{
+  return -getMass() * getWorldTransform().translation().dot(gravity);
 }
 
 //==============================================================================

diff --git a/dart/dynamics/BodyNode.hpp b/dart/dynamics/BodyNode.hpp
@@ -39,6 +39,7 @@
 #include <Eigen/StdVector>
 
 #include "dart/config.hpp"
+#include "dart/common/Deprecated.hpp"
 #include "dart/common/Signal.hpp"
 #include "dart/common/EmbeddedAspect.hpp"
 #include "dart/math/Geometry.hpp"
@@ -802,12 +803,23 @@ class BodyNode :
   // Energies
   //----------------------------------------------------------------------------
 
+  /// Return Lagrangian of this body
+  double computeLagrangian(const Eigen::Vector3d& gravity) const;
+
   /// Return kinetic energy.
+  DART_DEPRECATED(6.1)
   virtual double getKineticEnergy() const;
 
+  /// Return kinetic energy
+  double computeKineticEnergy() const;
+
   /// Return potential energy.
+  DART_DEPRECATED(6.1)
   virtual double getPotentialEnergy(const Eigen::Vector3d& _gravity) const;
 
+  /// Return potential energy.
+  double computePotentialEnergy(const Eigen::Vector3d& gravity) const;
+
   /// Return linear momentum.
   Eigen::Vector3d getLinearMomentum() const;
 

diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp
@@ -429,6 +429,12 @@ bool Joint::checkSanity(bool _printWarnings) const
   return sane;
 }
 
+//==============================================================================
+double Joint::getPotentialEnergy() const
+{
+  return computePotentialEnergy();
+}
+
 //==============================================================================
 void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T)
 {

diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp
@@ -534,7 +534,11 @@ class Joint : public virtual common::Subject,
   //----------------------------------------------------------------------------
 
   /// Get potential energy
-  virtual double getPotentialEnergy() const = 0;
+  DART_DEPRECATED(6.1)
+  double getPotentialEnergy() const;
+
+  /// Compute and return the potential energy
+  virtual double computePotentialEnergy() const = 0;
 
   //----------------------------------------------------------------------------
 

diff --git a/dart/dynamics/MetaSkeleton.cpp b/dart/dynamics/MetaSkeleton.cpp
@@ -937,6 +937,24 @@ Eigen::VectorXd MetaSkeleton::getJointConstraintImpulses() const
         this, "getJointConstraintImpulses");
 }
 
+//==============================================================================
+double MetaSkeleton::computeLagrangian() const
+{
+  return computeKineticEnergy() - computePotentialEnergy();
+}
+
+//==============================================================================
+double MetaSkeleton::getKineticEnergy() const
+{
+  return computeKineticEnergy();
+}
+
+//==============================================================================
+double MetaSkeleton::getPotentialEnergy() const
+{
+  return computePotentialEnergy();
+}
+
 //==============================================================================
 MetaSkeleton::MetaSkeleton()
   : onNameChanged(mNameChangedSignal)

diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp
@@ -646,11 +646,22 @@ class MetaSkeleton : public common::Subject
   /// Clear the internal forces of the BodyNodes in this MetaSkeleton
   virtual void clearInternalForces() = 0;
 
+  /// Compute and return Lagrangian of this MetaSkeleton
+  double computeLagrangian() const;
+
+  /// Get the kinetic energy of this MetaSkeleton
+  DART_DEPRECATED(6.1)
+  double getKineticEnergy() const;
+
   /// Get the kinetic energy of this MetaSkeleton
-  virtual double getKineticEnergy() const = 0;
+  virtual double computeKineticEnergy() const = 0;
+
+  /// Get the potential energy of this MetaSkeleton
+  DART_DEPRECATED(6.1)
+  double getPotentialEnergy() const;
 
   /// Get the potential energy of this MetaSkeleton
-  virtual double getPotentialEnergy() const = 0;
+  virtual double computePotentialEnergy() const = 0;
 
   /// Clear collision flags of the BodyNodes in this MetaSkeleton
   DART_DEPRECATED(6.0)

diff --git a/dart/dynamics/MultiDofJoint.hpp b/dart/dynamics/MultiDofJoint.hpp
@@ -410,7 +410,7 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint<DOF>, DOF
   //----------------------------------------------------------------------------
 
   // Documentation inherited
-  double getPotentialEnergy() const override;
+  double computePotentialEnergy() const override;
 
   // Documentation inherited
   Eigen::Vector6d getBodyConstraintWrench() const override;

diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp
@@ -758,26 +758,26 @@ void ReferentialSkeleton::clearInternalForces()
 }
 
 //==============================================================================
-double ReferentialSkeleton::getKineticEnergy() const
+double ReferentialSkeleton::computeKineticEnergy() const
 {
   double KE = 0.0;
 
   for(const BodyNode* bn : mRawBodyNodes)
-    KE += bn->getKineticEnergy();
+    KE += bn->computeKineticEnergy();
 
   assert( KE >= 0.0 && "Kinetic Energy should always be zero or greater");
   return KE;
 }
 
 //==============================================================================
-double ReferentialSkeleton::getPotentialEnergy() const
+double ReferentialSkeleton::computePotentialEnergy() const
 {
   double PE = 0.0;
 
   for(const BodyNode* bn : mRawBodyNodes)
   {
-    PE += bn->getPotentialEnergy(bn->getSkeleton()->getGravity());
-    PE += bn->getParentJoint()->getPotentialEnergy();
+    PE += bn->computePotentialEnergy(bn->getSkeleton()->getGravity());
+    PE += bn->getParentJoint()->computePotentialEnergy();
   }
 
   return PE;

diff --git a/dart/dynamics/ReferentialSkeleton.hpp b/dart/dynamics/ReferentialSkeleton.hpp
@@ -263,10 +263,10 @@ class ReferentialSkeleton : public MetaSkeleton
   void clearInternalForces() override;
 
   // Documentation inherited
-  double getKineticEnergy() const override;
+  double computeKineticEnergy() const override;
 
   // Documentation inherited
-  double getPotentialEnergy() const override;
+  double computePotentialEnergy() const override;
 
   // Documentation inherited
   DART_DEPRECATED(6.0)

diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp
@@ -1146,7 +1146,7 @@ double SingleDofJoint::getCoulombFriction(std::size_t _index) const
 }
 
 //==============================================================================
-double SingleDofJoint::getPotentialEnergy() const
+double SingleDofJoint::computePotentialEnergy() const
 {
   // Spring energy
   double pe = 0.5 * mAspectProperties.mSpringStiffness

diff --git a/dart/dynamics/SingleDofJoint.hpp b/dart/dynamics/SingleDofJoint.hpp
@@ -391,8 +391,8 @@ class SingleDofJoint : public detail::SingleDofJointBase
 
   //----------------------------------------------------------------------------
 
-  /// Get potential energy
-  double getPotentialEnergy() const override;
+  // Documentation inherited
+  double computePotentialEnergy() const override;
 
   // Documentation inherited
   const math::Jacobian getRelativeJacobian() const override;

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -3715,30 +3715,26 @@ void Skeleton::computeImpulseForwardDynamics()
 }
 
 //==============================================================================
-double Skeleton::getKineticEnergy() const
+double Skeleton::computeKineticEnergy() const
 {
   double KE = 0.0;
 
-  for (std::vector<BodyNode*>::const_iterator it = mSkelCache.mBodyNodes.begin();
-       it != mSkelCache.mBodyNodes.end(); ++it)
-  {
-    KE += (*it)->getKineticEnergy();
-  }
+  for (auto* bodyNode : mSkelCache.mBodyNodes)
+    KE += bodyNode->computeKineticEnergy();
 
   assert(KE >= 0.0 && "Kinetic energy should be positive value.");
   return KE;
 }
 
 //==============================================================================
-double Skeleton::getPotentialEnergy() const
+double Skeleton::computePotentialEnergy() const
 {
   double PE = 0.0;
 
-  for (std::vector<BodyNode*>::const_iterator it = mSkelCache.mBodyNodes.begin();
-       it != mSkelCache.mBodyNodes.end(); ++it)
+  for (auto* bodyNode : mSkelCache.mBodyNodes)
   {
-    PE += (*it)->getPotentialEnergy(mAspectProperties.mGravity);
-    PE += (*it)->getParentJoint()->getPotentialEnergy();
+    PE += bodyNode->computePotentialEnergy(mAspectProperties.mGravity);
+    PE += bodyNode->getParentJoint()->computePotentialEnergy();
   }
 
   return PE;

diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp
@@ -820,10 +820,10 @@ class Skeleton :
   void notifySupportUpdate(std::size_t _treeIdx);
 
   // Documentation inherited
-  double getKineticEnergy() const override;
+  double computeKineticEnergy() const override;
 
   // Documentation inherited
-  double getPotentialEnergy() const override;
+  double computePotentialEnergy() const override;
 
   // Documentation inherited
   DART_DEPRECATED(6.0)

diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp
@@ -556,7 +556,7 @@ double ZeroDofJoint::getCoulombFriction(std::size_t /*_index*/) const
 }
 
 //==============================================================================
-double ZeroDofJoint::getPotentialEnergy() const
+double ZeroDofJoint::computePotentialEnergy() const
 {
   return 0.0;
 }

diff --git a/dart/dynamics/ZeroDofJoint.hpp b/dart/dynamics/ZeroDofJoint.hpp
@@ -339,8 +339,8 @@ class ZeroDofJoint : public Joint
 
   //----------------------------------------------------------------------------
 
-  /// Get potential energy
-  double getPotentialEnergy() const override;
+  // Documentation inherited
+  double computePotentialEnergy() const override;
 
   // Documentation inherited
   Eigen::Vector6d getBodyConstraintWrench() const override;

diff --git a/dart/dynamics/detail/MultiDofJoint.hpp b/dart/dynamics/detail/MultiDofJoint.hpp
@@ -1320,7 +1320,7 @@ double MultiDofJoint<DOF>::getCoulombFriction(std::size_t _index) const
 
 //==============================================================================
 template <std::size_t DOF>
-double MultiDofJoint<DOF>::getPotentialEnergy() const
+double MultiDofJoint<DOF>::computePotentialEnergy() const
 {
   // Spring energy
   Eigen::VectorXd displacement =