[C++] New algo to retrieve the increment for finite differences

CMakeLists.txt

@@ -173,6 +173,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/rnea.hxx
   algorithm/crba.hpp
   algorithm/crba.hxx
+  algorithm/finite-differences.hpp
+  algorithm/finite-differences.hxx
   algorithm/jacobian.hpp
   algorithm/jacobian.hxx
   algorithm/cholesky.hpp

src/algorithm/finite-differences.hpp

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_finite_differences_hpp__
+#define __se3_finite_differences_hpp__
+
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3 {
+  
+  ///
+  /// \brief Computes the finite difference increments for each degree of freedom according to the current joint configuration.
+  ///
+  /// \input[in] model The model of the kinematic tree.
+  ///
+  /// \returns The finite difference increments for each degree of freedom.
+  ///
+  inline Eigen::VectorXd finiteDifferenceIncrement(const Model & model);
+}
+
+/* --- Details -------------------------------------------------------------------- */
+#include "pinocchio/algorithm/finite-differences.hxx"
+
+#endif // ifndef __se3_finite_differences_hpp__

src/algorithm/finite-differences.hxx

+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_finite_differences_hxx__
+#define __se3_finite_differences_hxx__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include <boost/foreach.hpp>
+
+/// @cond DEV
+
+namespace se3
+{
+  namespace details
+  {
+    struct FinitDiffEpsVisitor : public fusion::JointModelVisitor<FinitDiffEpsVisitor>
+    {
+      typedef boost::fusion::vector<
+      Eigen::VectorXd &
+      > ArgsType;
+      
+      JOINT_MODEL_VISITOR_INIT(FinitDiffEpsVisitor);
+      
+      template<typename JointModel>
+      static void algo(const se3::JointModelBase<JointModel> & jmodel,
+                       Eigen::VectorXd & fd_increment)
+      {
+        jmodel.jointVelocitySelector(fd_increment).fill(jmodel.finiteDifferenceIncrement());
+      }
+      
+    }; // struct FinitDiffEpsVisitor
+    
+  } // namespace details
+  
+  inline Eigen::VectorXd finiteDifferenceIncrement(const Model & model)
+  {
+    using namespace se3::details;
+    Eigen::VectorXd fd_increment(model.nv);
+    BOOST_FOREACH(const JointModel & jmodel,model.joints)
+    {
+      FinitDiffEpsVisitor::run(jmodel,FinitDiffEpsVisitor::ArgsType(fd_increment));
+    }
+    
+    return fd_increment;
+  }
+} // namespace se3
+
+/// @endcond
+
+#endif // ifndef __se3_finite_differences_hxx__