Change Function::eval[Gradient/Hessian] to const functions (#928)

CHANGELOG.md

@@ -1,5 +1,33 @@
+## DART 7
+
+### DART 7.0.0 (201X-XX-XX)
+
+* Optimizer
+
+  * Changed Function::eval\[Gradient/Hessian\](...) to const function: [#928](https://github.com/dartsim/dart/pull/928)
+
 ## DART 6
 
+### DART 6.4.0 (201X-XX-XX)
+
+### [DART 6.3.0 (2017-10-04)](https://github.com/dartsim/dart/milestone/36?closed=1)
+
+* Collision detection
+
+  * Added a feature of disabling body node pairs to BodyNodeCollisionFilter: [#911](https://github.com/dartsim/dart/pull/911)
+
+* Kinematics/Dynamics
+
+  * Added setter and getter for WeldJointConstraint::mRelativeTransform: [#910](https://github.com/dartsim/dart/pull/910)
+
+* Parsers
+
+  * Improved SkelParser to read alpha value: [#914](https://github.com/dartsim/dart/pull/914)
+
+* Misc
+
+  * Changed not to use lambda function as an workaround for DART python binding: [#916](https://github.com/dartsim/dart/pull/916)
+
 ### [DART 6.2.1 (2017-08-08)](https://github.com/dartsim/dart/milestone/37?closed=1)
 
 * Collision detection

dart/constraint/BalanceConstraint.cpp

@@ -61,7 +61,7 @@ optimizer::FunctionPtr BalanceConstraint::clone(
 }
 
 //==============================================================================
-double BalanceConstraint::eval(const Eigen::VectorXd& _x)
+double BalanceConstraint::eval(const Eigen::VectorXd& _x) const
 {
   const std::shared_ptr<dynamics::HierarchicalIK>& ik = mIK.lock();
 
@@ -177,7 +177,7 @@ static void addDampedPseudoInverseToGradient(
 
 //==============================================================================
 void BalanceConstraint::evalGradient(const Eigen::VectorXd& _x,
-                                     Eigen::Map<Eigen::VectorXd> _grad)
+                                     Eigen::Map<Eigen::VectorXd> _grad) const
 {
   _grad.setZero();
   if(eval(_x) == 0.0)
@@ -399,7 +399,7 @@ void BalanceConstraint::clearCaches()
 
 //==============================================================================
 void BalanceConstraint::convertJacobianMethodOutputToGradient(
-    Eigen::Map<Eigen::VectorXd>& grad)
+    Eigen::Map<Eigen::VectorXd>& grad) const
 {
   const dart::dynamics::SkeletonPtr& skel = mIK.lock()->getSkeleton();
   skel->setVelocities(grad);

dart/constraint/BalanceConstraint.hpp

@@ -96,11 +96,11 @@ class BalanceConstraint : public optimizer::Function,
       const std::shared_ptr<dynamics::HierarchicalIK>& _newIK) const override;
 
   // Documentation inherited
-  double eval(const Eigen::VectorXd& _x) override;
+  double eval(const Eigen::VectorXd& _x) const override;
 
   // Documentation inherited
   void evalGradient(const Eigen::VectorXd& _x,
-                    Eigen::Map<Eigen::VectorXd> _grad) override;
+                    Eigen::Map<Eigen::VectorXd> _grad) const override;
 
   /// Set the method that this constraint function will use to compute the
   /// error. See the ErrorMethod_t docs for more information.
@@ -143,7 +143,7 @@ class BalanceConstraint : public optimizer::Function,
   /// Convert the gradient that gets generated via Jacobian methods into a
   /// gradient that can be used by a GradientDescentSolver.
   void convertJacobianMethodOutputToGradient(
-      Eigen::Map<Eigen::VectorXd>& grad);
+      Eigen::Map<Eigen::VectorXd>& grad) const;
 
   /// Pointer to the hierarchical IK that owns this Function. Note that this
   /// Function does not work correctly without a HierarchicalIK.
@@ -163,11 +163,11 @@ class BalanceConstraint : public optimizer::Function,
 
   /// The indices of the supporting end effectors that are closest to the center
   /// of mass. These are used when using FROM_EDGE
-  std::size_t mClosestEndEffector[2];
+  mutable std::size_t mClosestEndEffector[2];
 
   /// The error vector points away from the direction that the center of mass
   /// should move in order to reduce the balance error
-  Eigen::Vector3d mLastError;
+  mutable Eigen::Vector3d mLastError;
 
   /// The last computed location of the center of mass
   Eigen::Vector3d mLastCOM;
@@ -177,24 +177,24 @@ class BalanceConstraint : public optimizer::Function,
 
   /// Cache for the center of mass Jacobian so that the memory space does not
   /// need to be reallocated each loop
-  math::LinearJacobian mComJacCache;
+  mutable math::LinearJacobian mComJacCache;
 
   /// Cache for the end effector Jacobians so the space does not need to be
   /// reallocated each loop
-  math::LinearJacobian mEEJacCache;
+  mutable math::LinearJacobian mEEJacCache;
 
   /// Cache for the SVD
-  Eigen::JacobiSVD<math::LinearJacobian> mSVDCache;
+  mutable Eigen::JacobiSVD<math::LinearJacobian> mSVDCache;
 
   /// Cache for the full null space
-  Eigen::MatrixXd mNullSpaceCache;
+  mutable Eigen::MatrixXd mNullSpaceCache;
 
   /// Cache for an individual null space
-  Eigen::MatrixXd mPartialNullSpaceCache;
+  mutable Eigen::MatrixXd mPartialNullSpaceCache;
 
   /// Cache used by convertJacobianMethodOutputToGradient to avoid reallocating
   /// this vector on each iteration.
-  Eigen::VectorXd mInitialPositionsCache;
+  mutable Eigen::VectorXd mInitialPositionsCache;
 };
 
 } // namespace constraint

dart/dynamics/HierarchicalIK.cpp

@@ -366,7 +366,7 @@ optimizer::FunctionPtr HierarchicalIK::Objective::clone(
 }
 
 //==============================================================================
-double HierarchicalIK::Objective::eval(const Eigen::VectorXd& _x)
+double HierarchicalIK::Objective::eval(const Eigen::VectorXd& _x) const
 {
   const std::shared_ptr<HierarchicalIK>& hik = mIK.lock();
 
@@ -391,7 +391,7 @@ double HierarchicalIK::Objective::eval(const Eigen::VectorXd& _x)
 
 //==============================================================================
 void HierarchicalIK::Objective::evalGradient(
-    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
+    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) const
 {
   const std::shared_ptr<HierarchicalIK>& hik = mIK.lock();
 
@@ -441,7 +441,7 @@ optimizer::FunctionPtr HierarchicalIK::Constraint::clone(const std::shared_ptr<H
 }
 
 //==============================================================================
-double HierarchicalIK::Constraint::eval(const Eigen::VectorXd& _x)
+double HierarchicalIK::Constraint::eval(const Eigen::VectorXd& _x) const
 {
   const std::shared_ptr<HierarchicalIK>& hik = mIK.lock();
   if(nullptr == hik)
@@ -484,7 +484,7 @@ double HierarchicalIK::Constraint::eval(const Eigen::VectorXd& _x)
 
 //==============================================================================
 void HierarchicalIK::Constraint::evalGradient(
-    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
+    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) const
 {
   const std::shared_ptr<HierarchicalIK>& hik = mIK.lock();
 

dart/dynamics/HierarchicalIK.hpp

@@ -201,19 +201,19 @@ class HierarchicalIK : public common::Subject
         const std::shared_ptr<HierarchicalIK>& _newIK) const override;
 
     // Documentation inherited
-    double eval(const Eigen::VectorXd &_x) override;
+    double eval(const Eigen::VectorXd &_x) const override;
 
     // Documentation inherited
     void evalGradient(const Eigen::VectorXd& _x,
-                      Eigen::Map<Eigen::VectorXd> _grad) override;
+                      Eigen::Map<Eigen::VectorXd> _grad) const override;
 
   protected:
 
     /// Pointer to this Objective's HierarchicalIK module
     std::weak_ptr<HierarchicalIK> mIK;
 
     /// Cache for the gradient computation
-    Eigen::VectorXd mGradCache;
+    mutable Eigen::VectorXd mGradCache;
   };
 
   /// The HierarchicalIK::Constraint Function is simply used to merge the
@@ -236,22 +236,22 @@ class HierarchicalIK : public common::Subject
         const std::shared_ptr<HierarchicalIK>& _newIK) const override;
 
     // Documentation inherited
-    double eval(const Eigen::VectorXd& _x) override;
+    double eval(const Eigen::VectorXd& _x) const override;
 
     // Documentation inherited
     void evalGradient(const Eigen::VectorXd& _x,
-                      Eigen::Map<Eigen::VectorXd> _grad) override;
+                      Eigen::Map<Eigen::VectorXd> _grad) const override;
 
   protected:
 
     /// Pointer to this Constraint's HierarchicalIK module
     std::weak_ptr<HierarchicalIK> mIK;
 
     /// Cache for the gradient of a level
-    Eigen::VectorXd mLevelGradCache;
+    mutable Eigen::VectorXd mLevelGradCache;
 
     /// Cache for temporary gradients
-    Eigen::VectorXd mTempGradCache;
+    mutable Eigen::VectorXd mTempGradCache;
   };
 
   /// Constructor

dart/dynamics/InverseKinematics.cpp

@@ -1631,7 +1631,7 @@ optimizer::FunctionPtr InverseKinematics::Objective::clone(
 }
 
 //==============================================================================
-double InverseKinematics::Objective::eval(const Eigen::VectorXd& _x)
+double InverseKinematics::Objective::eval(const Eigen::VectorXd& _x) const
 {
   if(nullptr == mIK)
   {
@@ -1654,7 +1654,7 @@ double InverseKinematics::Objective::eval(const Eigen::VectorXd& _x)
 
 //==============================================================================
 void InverseKinematics::Objective::evalGradient(
-    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
+    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) const
 {
   if(nullptr == mIK)
   {
@@ -1699,7 +1699,7 @@ optimizer::FunctionPtr InverseKinematics::Constraint::clone(
 }
 
 //==============================================================================
-double InverseKinematics::Constraint::eval(const Eigen::VectorXd& _x)
+double InverseKinematics::Constraint::eval(const Eigen::VectorXd& _x) const
 {
   if(nullptr == mIK)
   {
@@ -1714,7 +1714,7 @@ double InverseKinematics::Constraint::eval(const Eigen::VectorXd& _x)
 
 //==============================================================================
 void InverseKinematics::Constraint::evalGradient(
-    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
+    const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) const
 {
   if(nullptr == mIK)
   {

dart/dynamics/InverseKinematics.hpp

@@ -1234,26 +1234,26 @@ class InverseKinematics::Objective final :
   optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
 
   // Documentation inherited
-  double eval(const Eigen::VectorXd& _x) override;
+  double eval(const Eigen::VectorXd& _x) const override;
 
   // Documentation inherited
   void evalGradient(const Eigen::VectorXd& _x,
-                    Eigen::Map<Eigen::VectorXd> _grad) override;
+                    Eigen::Map<Eigen::VectorXd> _grad) const override;
 
 protected:
 
   /// Pointer to this Objective's IK module
   sub_ptr<InverseKinematics> mIK;
 
   /// Cache for the gradient of the Objective
-  Eigen::VectorXd mGradCache;
+  mutable Eigen::VectorXd mGradCache;
 
   /// Cache for the null space SVD
-  Eigen::JacobiSVD<math::Jacobian> mSVDCache;
+  mutable Eigen::JacobiSVD<math::Jacobian> mSVDCache;
   // TODO(JS): Need to define aligned operator new for this?
 
   /// Cache for the null space
-  Eigen::MatrixXd mNullSpaceCache;
+  mutable Eigen::MatrixXd mNullSpaceCache;
 
 public:
   // To get byte-aligned Eigen vectors
@@ -1282,11 +1282,11 @@ class InverseKinematics::Constraint final :
   optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
 
   // Documentation inherited
-  double eval(const Eigen::VectorXd& _x) override;
+  double eval(const Eigen::VectorXd& _x) const override;
 
   // Documentation inherited
   void evalGradient(const Eigen::VectorXd& _x,
-                    Eigen::Map<Eigen::VectorXd> _grad) override;
+                    Eigen::Map<Eigen::VectorXd> _grad) const override;
 
 protected:
 

dart/math/Geometry.hpp

@@ -466,8 +466,10 @@ inline double wrapToPi(double angle)
 template <typename MatrixType, typename ReturnType>
 void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
 {
+#if EIGEN_VERSION_AT_LEAST(3,2,2)
+  const int rank = _SVD.rank();
+#else
   int rank = 0;
-  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
   if(_SVD.nonzeroSingularValues() > 0)
   {
     double thresh = std::max(_SVD.singularValues().coeff(0)*1e-10,
@@ -477,8 +479,10 @@ void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
       --i;
     rank = i+1;
   }
+#endif
 
-  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
+  const int cols = _SVD.matrixV().cols();
+  const int rows = _SVD.matrixV().rows();
   _NS = _SVD.matrixV().block(0, rank, rows, cols-rank);
 }