Merge remote-tracking branch 'origin/release-6.1'

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)
@@ -10,6 +14,7 @@
 
 * Collision detection
 
+  * Added support of FCL 0.5.0: [#749](https://github.com/dartsim/dart/pull/749)
   * Added warnings for unsupported shape pairs of DARTCollisionDetector: [#722](https://github.com/dartsim/dart/pull/722)
 
 * Misc improvements and bug fixes

dart/collision/fcl/FCLCollisionDetector.cpp

@@ -431,8 +431,6 @@ fcl::BVHModel<BV>* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ)
   return model;
 }
 
-#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4)
-
 //==============================================================================
 template<class BV>
 fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
@@ -536,8 +534,6 @@ fcl::BVHModel<BV>* createCylinder(double _baseRadius, double _topRadius,
   return model;
 }
 
-#endif
-
 //==============================================================================
 template<class BV>
 fcl::BVHModel<BV>* createMesh(float _scaleX, float _scaleY, float _scaleZ,
@@ -782,7 +778,7 @@ std::unique_ptr<CollisionObject> FCLCollisionDetector::createCollisionObject(
 }
 
 //==============================================================================
-boost::shared_ptr<fcl::CollisionGeometry>
+fcl_shared_ptr<fcl::CollisionGeometry>
 FCLCollisionDetector::claimFCLCollisionGeometry(
     const dynamics::ConstShapePtr& shape)
 {
@@ -805,7 +801,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry(
 }
 
 //==============================================================================
-boost::shared_ptr<fcl::CollisionGeometry>
+fcl_shared_ptr<fcl::CollisionGeometry>
 FCLCollisionDetector::createFCLCollisionGeometry(
     const dynamics::ConstShapePtr& shape,
     FCLCollisionDetector::PrimitiveShape type,
@@ -930,7 +926,7 @@ FCLCollisionDetector::createFCLCollisionGeometry(
     geom = createEllipsoid<fcl::OBBRSS>(0.1, 0.1, 0.1);
   }
 
-  return boost::shared_ptr<fcl::CollisionGeometry>(geom, deleter);
+  return fcl_shared_ptr<fcl::CollisionGeometry>(geom, deleter);
 }
 
 //==============================================================================

dart/collision/fcl/FCLCollisionDetector.hpp

@@ -36,6 +36,7 @@
 #include <fcl/collision_object.h>
 #include <boost/weak_ptr.hpp> // This should be removed once we migrate to fcl 0.5
 #include "dart/collision/CollisionDetector.hpp"
+#include "dart/collision/fcl/FCLTypes.hpp"
 
 namespace dart {
 namespace collision {
@@ -130,7 +131,7 @@ class FCLCollisionDetector : public CollisionDetector
 
   /// Return fcl::CollisionGeometry associated with give Shape. New
   /// fcl::CollisionGeome will be created if it hasn't created yet.
-  boost::shared_ptr<fcl::CollisionGeometry> claimFCLCollisionGeometry(
+  fcl_shared_ptr<fcl::CollisionGeometry> claimFCLCollisionGeometry(
       const dynamics::ConstShapePtr& shape);
 
 protected:
@@ -162,15 +163,15 @@ class FCLCollisionDetector : public CollisionDetector
 
   /// Create fcl::CollisionGeometry with the custom deleter
   /// FCLCollisionGeometryDeleter
-  boost::shared_ptr<fcl::CollisionGeometry> createFCLCollisionGeometry(
+  fcl_shared_ptr<fcl::CollisionGeometry> createFCLCollisionGeometry(
       const dynamics::ConstShapePtr& shape,
       FCLCollisionDetector::PrimitiveShape type,
       const FCLCollisionGeometryDeleter& deleter);
 
 private:
 
   using ShapeMap = std::map<dynamics::ConstShapePtr,
-                            boost::weak_ptr<fcl::CollisionGeometry>>;
+                            fcl_weak_ptr<fcl::CollisionGeometry>>;
   // TODO(JS): FCL replaced all the use of boost in version 0.5. Once we migrate
   // to 0.5 or greater, this also should be changed to
   // std::weak_ptr<fcl::CollisionGeometry>

dart/collision/fcl/FCLCollisionObject.cpp

@@ -63,7 +63,7 @@ const fcl::CollisionObject* FCLCollisionObject::getFCLCollisionObject() const
 FCLCollisionObject::FCLCollisionObject(
     CollisionDetector* collisionDetector,
     const dynamics::ShapeFrame* shapeFrame,
-    const boost::shared_ptr<fcl::CollisionGeometry>& fclCollGeom)
+    const fcl_shared_ptr<fcl::CollisionGeometry>& fclCollGeom)
   : CollisionObject(collisionDetector, shapeFrame),
     mFCLCollisionObjectUserData(new UserData(this)),
     mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom))

dart/collision/fcl/FCLCollisionObject.hpp

@@ -34,6 +34,7 @@
 
 #include <fcl/collision_object.h>
 #include "dart/collision/CollisionObject.hpp"
+#include "dart/collision/fcl/FCLTypes.hpp"
 
 namespace dart {
 namespace collision {
@@ -64,7 +65,7 @@ class FCLCollisionObject : public CollisionObject
   /// Constructor
   FCLCollisionObject(CollisionDetector* collisionDetector,
       const dynamics::ShapeFrame* shapeFrame,
-      const boost::shared_ptr<fcl::CollisionGeometry>& fclCollGeom);
+      const fcl_shared_ptr<fcl::CollisionGeometry>& fclCollGeom);
 
   // Documentation inherited
   void updateEngineData() override;

dart/collision/fcl/FCLTypes.hpp

@@ -46,6 +46,14 @@
   (FCL_MAJOR_VERSION < x || (FCL_MAJOR_VERSION <= x && \
   (FCL_MINOR_VERSION < y || (FCL_MINOR_VERSION <= y))))
 
+#if FCL_VERSION_AT_LEAST(0,5,0)
+template <class T> using fcl_shared_ptr = std::shared_ptr<T>;
+template <class T> using fcl_weak_ptr = std::weak_ptr<T>;
+#else
+template <class T> using fcl_shared_ptr = boost::shared_ptr<T>;
+template <class T> using fcl_weak_ptr = boost::weak_ptr<T>;
+#endif
+
 namespace dart {
 namespace collision {
 

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);
 }
 
 //==============================================================================

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;
 

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)
 {

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;
 
   //----------------------------------------------------------------------------
 

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)

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)

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;

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;

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)

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

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;