From 2841967fc6628c9aea58c93227b28abdf5dfbcc2 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 17:34:40 -0400 Subject: [PATCH 1/2] Update API to support fcl 0.5 and tinyxml 4.0 -- for DART 6.1 (#750) * Update API to support fcl 0.5.0 * Update changelog * Use type aliasing rather than boilerplate preprocessors * Switch to use XML_SUCCESS rather than XML_NO_ERROR --- CHANGELOG.md | 1 + dart/collision/fcl/FCLCollisionDetector.cpp | 10 +++------- dart/collision/fcl/FCLCollisionDetector.hpp | 7 ++++--- dart/collision/fcl/FCLCollisionObject.cpp | 2 +- dart/collision/fcl/FCLCollisionObject.hpp | 3 ++- dart/collision/fcl/FCLTypes.hpp | 8 ++++++++ dart/utils/XmlHelpers.cpp | 10 +++++----- 7 files changed, 24 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ac756d7cb5a30..945e1b8606cc9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,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 diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 6d5173c060b05..c746cd4944428 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -431,8 +431,6 @@ fcl::BVHModel* createEllipsoid(float _sizeX, float _sizeY, float _sizeZ) return model; } -#if FCL_MAJOR_MINOR_VERSION_AT_MOST(0,4) - //============================================================================== template fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, @@ -536,8 +534,6 @@ fcl::BVHModel* createCylinder(double _baseRadius, double _topRadius, return model; } -#endif - //============================================================================== template fcl::BVHModel* createMesh(float _scaleX, float _scaleY, float _scaleZ, @@ -782,7 +778,7 @@ std::unique_ptr FCLCollisionDetector::createCollisionObject( } //============================================================================== -boost::shared_ptr +fcl_shared_ptr FCLCollisionDetector::claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape) { @@ -805,7 +801,7 @@ FCLCollisionDetector::claimFCLCollisionGeometry( } //============================================================================== -boost::shared_ptr +fcl_shared_ptr FCLCollisionDetector::createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, @@ -930,7 +926,7 @@ FCLCollisionDetector::createFCLCollisionGeometry( geom = createEllipsoid(0.1, 0.1, 0.1); } - return boost::shared_ptr(geom, deleter); + return fcl_shared_ptr(geom, deleter); } //============================================================================== diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index b808619022801..d37c0e58ccb8f 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -36,6 +36,7 @@ #include #include // 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 claimFCLCollisionGeometry( + fcl_shared_ptr claimFCLCollisionGeometry( const dynamics::ConstShapePtr& shape); protected: @@ -162,7 +163,7 @@ class FCLCollisionDetector : public CollisionDetector /// Create fcl::CollisionGeometry with the custom deleter /// FCLCollisionGeometryDeleter - boost::shared_ptr createFCLCollisionGeometry( + fcl_shared_ptr createFCLCollisionGeometry( const dynamics::ConstShapePtr& shape, FCLCollisionDetector::PrimitiveShape type, const FCLCollisionGeometryDeleter& deleter); @@ -170,7 +171,7 @@ class FCLCollisionDetector : public CollisionDetector private: using ShapeMap = std::map>; + fcl_weak_ptr>; // 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 diff --git a/dart/collision/fcl/FCLCollisionObject.cpp b/dart/collision/fcl/FCLCollisionObject.cpp index db77466280d0a..bf4a682550cf0 100644 --- a/dart/collision/fcl/FCLCollisionObject.cpp +++ b/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& fclCollGeom) + const fcl_shared_ptr& fclCollGeom) : CollisionObject(collisionDetector, shapeFrame), mFCLCollisionObjectUserData(new UserData(this)), mFCLCollisionObject(new fcl::CollisionObject(fclCollGeom)) diff --git a/dart/collision/fcl/FCLCollisionObject.hpp b/dart/collision/fcl/FCLCollisionObject.hpp index ff011cf75e074..544c36eea1fbb 100644 --- a/dart/collision/fcl/FCLCollisionObject.hpp +++ b/dart/collision/fcl/FCLCollisionObject.hpp @@ -34,6 +34,7 @@ #include #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& fclCollGeom); + const fcl_shared_ptr& fclCollGeom); // Documentation inherited void updateEngineData() override; diff --git a/dart/collision/fcl/FCLTypes.hpp b/dart/collision/fcl/FCLTypes.hpp index 8fa6e88108a81..8fbf15c2c130f 100644 --- a/dart/collision/fcl/FCLTypes.hpp +++ b/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 using fcl_shared_ptr = std::shared_ptr; +template using fcl_weak_ptr = std::weak_ptr; +#else +template using fcl_shared_ptr = boost::shared_ptr; +template using fcl_weak_ptr = boost::weak_ptr; +#endif + namespace dart { namespace collision { diff --git a/dart/utils/XmlHelpers.cpp b/dart/utils/XmlHelpers.cpp index adaf7de17647f..bb0fcc7ca329e 100644 --- a/dart/utils/XmlHelpers.cpp +++ b/dart/utils/XmlHelpers.cpp @@ -726,7 +726,7 @@ bool getAttributeBool(const tinyxml2::XMLElement* element, const int result = element->QueryBoolAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing bool type attribute [" << attributeName << "] of an element [" << element->Name() @@ -744,7 +744,7 @@ int getAttributeInt(const tinyxml2::XMLElement* element, int val = 0; const int result = element->QueryIntAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -763,7 +763,7 @@ unsigned int getAttributeUInt(const tinyxml2::XMLElement* element, const int result = element->QueryUnsignedAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing unsiged int type attribute [" << attributeName << "] of an element [" << element->Name() @@ -782,7 +782,7 @@ float getAttributeFloat(const tinyxml2::XMLElement* element, const int result = element->QueryFloatAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing float type attribute [" << attributeName << "] of an element [" << element->Name() @@ -801,7 +801,7 @@ double getAttributeDouble(const tinyxml2::XMLElement* element, const int result = element->QueryDoubleAttribute(attributeName.c_str(), &val); - if (result != tinyxml2::XML_NO_ERROR) + if (result != tinyxml2::XML_SUCCESS) { dtwarn << "[getAttribute] Error in parsing double type attribute [" << attributeName << "] of an element [" << element->Name() From ef41e05ba41eef153194be9e027a56e7b5a59713 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Jul 2016 22:34:09 -0400 Subject: [PATCH 2/2] Add computeLagrangian() to MetaSkeleton and BodyNode (#746) * Add computeLagrangian() to MetaSkeleton and BodyNode * Fix incorrect deprecated macros * Update API to support fcl 0.5.0 * Update changelog * Update changelog * Use type aliasing rather than boilerplate preprocessors * Switch to use XML_SUCCESS rather than XML_NO_ERROR --- CHANGELOG.md | 4 ++++ dart/dynamics/BodyNode.cpp | 25 ++++++++++++++++++++++--- dart/dynamics/BodyNode.hpp | 12 ++++++++++++ dart/dynamics/Joint.cpp | 6 ++++++ dart/dynamics/Joint.hpp | 6 +++++- dart/dynamics/MetaSkeleton.cpp | 18 ++++++++++++++++++ dart/dynamics/MetaSkeleton.hpp | 15 +++++++++++++-- dart/dynamics/MultiDofJoint.hpp | 2 +- dart/dynamics/ReferentialSkeleton.cpp | 10 +++++----- dart/dynamics/ReferentialSkeleton.hpp | 4 ++-- dart/dynamics/SingleDofJoint.cpp | 2 +- dart/dynamics/SingleDofJoint.hpp | 4 ++-- dart/dynamics/Skeleton.cpp | 18 +++++++----------- dart/dynamics/Skeleton.hpp | 4 ++-- dart/dynamics/ZeroDofJoint.cpp | 2 +- dart/dynamics/ZeroDofJoint.hpp | 4 ++-- dart/dynamics/detail/MultiDofJoint.hpp | 2 +- 17 files changed, 104 insertions(+), 34 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 945e1b8606cc9..5b1016f714448 100644 --- 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 index 07ae155565586..edea193e256ab 100644 --- 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 index 590853bdf2a1b..5be87b01648c5 100644 --- a/dart/dynamics/BodyNode.hpp +++ b/dart/dynamics/BodyNode.hpp @@ -39,6 +39,7 @@ #include #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 index c5fb91e885b44..5d3ede9b536e8 100644 --- 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 index 40161e1f42068..3cbe033f0765e 100644 --- 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 index 8497d924238f0..1828999712a59 100644 --- 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 index 97c0336826c94..6146b8d58e9f3 100644 --- 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 index fadd68c05e529..1aa8152b519b1 100644 --- a/dart/dynamics/MultiDofJoint.hpp +++ b/dart/dynamics/MultiDofJoint.hpp @@ -410,7 +410,7 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, 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 index a57a298d6fe55..005cfa1d61662 100644 --- 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 index 7890e5a1cc7c0..f0a7f6ba587f8 100644 --- 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 index f2b95508cd3de..bfce40a479049 100644 --- 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 index 8092cbcc37804..6e679756759bb 100644 --- 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 index f6437e42e01b2..5aa5bb0952e94 100644 --- 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::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::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 index 172a4422ebb1f..ac9e56cbbb749 100644 --- 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 index 9a5e0ac3c9d22..2fa563419c75a 100644 --- 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 index 28d7ed2752a03..0a47758ce59f3 100644 --- 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 index 8f9b600d4b9bf..966fc27c34e43 100644 --- a/dart/dynamics/detail/MultiDofJoint.hpp +++ b/dart/dynamics/detail/MultiDofJoint.hpp @@ -1320,7 +1320,7 @@ double MultiDofJoint::getCoulombFriction(std::size_t _index) const //============================================================================== template -double MultiDofJoint::getPotentialEnergy() const +double MultiDofJoint::computePotentialEnergy() const { // Spring energy Eigen::VectorXd displacement =