finite-differences.hxx 1.94 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
//
// 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);
53
    for(std::size_t k = 1; k < model.joints.size(); ++k)
54
    {
55
      const JointModel & jmodel = model.joints[k];
56
57
58
59
60
61
62
63
64
65
      FinitDiffEpsVisitor::run(jmodel,FinitDiffEpsVisitor::ArgsType(fd_increment));
    }
    
    return fd_increment;
  }
} // namespace se3

/// @endcond

#endif // ifndef __se3_finite_differences_hxx__