vector-space.hpp 6.5 KB
Newer Older
1
//
Justin Carpentier's avatar
Justin Carpentier committed
2
// Copyright (c) 2016-2020 CNRS INRIA
3
4
//

5
6
#ifndef __pinocchio_vector_space_operation_hpp__
#define __pinocchio_vector_space_operation_hpp__
7

8
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9

Justin Carpentier's avatar
Justin Carpentier committed
10
#include <stdexcept>
11
#include <boost/integer/static_min_max.hpp>
12

13
namespace pinocchio
14
{
15
16
17
18
19
20
  template<int Dim, typename Scalar, int Options = 0> struct VectorSpaceOperationTpl;
  
  template<int Dim, typename _Scalar, int _Options>
  struct traits< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
21
    enum {
22
      Options = _Options,
23
24
      NQ = Dim,
      NV = Dim
25
26
27
    };
  };

28
29
30
  template<int Dim, typename _Scalar, int _Options>
  struct VectorSpaceOperationTpl
  : public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
31
  {
gabrielebndn's avatar
gabrielebndn committed
32
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);
33
34
35
36

    /// Constructor
    /// \param size size of the vector space: should be the equal to template
    ///        argument for static sized vector-spaces.
37
38
    VectorSpaceOperationTpl(int size = boost::static_signed_max<0,Dim>::value)
    : size_(size)
39
    {
40
      assert(size_.value() >= 0);
41
42
    }

43
    /// Constructor
44
    /// \param other other VectorSpaceOperationTpl from which to retrieve size
45
46
    VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
    : Base(), size_(other.size_.value())
47
    {
48
      assert(size_.value() >= 0);
49
50
    }

51
    Index nq () const
52
    {
53
      return size_.value();
54
    }
55
    Index nv () const
56
    {
57
      return size_.value();
58
    }
59

60
61
    ConfigVector_t neutral () const
    {
Qiang Qiu's avatar
Qiang Qiu committed
62
      return ConfigVector_t::Zero(size_.value());
63
64
    }

65
66
    std::string name () const
    {
67
      std::ostringstream oss; oss << "R^" << nq();
68
69
70
      return oss.str ();
    }

71
72
73
74
75
    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1,
                                const Eigen::MatrixBase<Tangent_t> & d)
    {
gabrielebndn's avatar
gabrielebndn committed
76
      PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
77
78
    }

79
    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
80
81
    void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
                           const Eigen::MatrixBase<ConfigR_t> &,
Justin Carpentier's avatar
Justin Carpentier committed
82
                           const Eigen::MatrixBase<JacobianOut_t> & J) const
83
    {
84
      if (arg == ARG0)
Justin Carpentier's avatar
Justin Carpentier committed
85
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity();
86
      else if (arg == ARG1)
gabrielebndn's avatar
gabrielebndn committed
87
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
88
89
    }

90
91
92
93
94
    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                               const Eigen::MatrixBase<Velocity_t> & v,
                               const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
gabrielebndn's avatar
gabrielebndn committed
95
      PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
96
97
    }

98
99
100
    template <class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> &,
                                                const Eigen::MatrixBase<Jacobian_t> & J)
101
    {
gabrielebndn's avatar
gabrielebndn committed
102
      PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
103
104
    }

105
    template <class Config_t, class Tangent_t, class JacobianOut_t>
Justin Carpentier's avatar
Justin Carpentier committed
106
107
108
    static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
109
                                   const AssignmentOperatorType op)
110
    {
111
112
113
114
115
116
117
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
Justin Carpentier's avatar
Justin Carpentier committed
118
          Jout.diagonal().array() += Scalar(1);
119
120
          break;
        case RMTO:
Justin Carpentier's avatar
Justin Carpentier committed
121
          Jout.diagonal().array() -= Scalar(1);
122
123
124
125
126
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
127
128
129
    }

    template <class Config_t, class Tangent_t, class JacobianOut_t>
Justin Carpentier's avatar
Justin Carpentier committed
130
131
132
    static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
                                   const Eigen::MatrixBase<Tangent_t> & /*v*/,
                                   const Eigen::MatrixBase<JacobianOut_t> & J,
133
                                   const AssignmentOperatorType op)
134
    {
135
136
137
138
139
140
141
      Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
      switch(op)
        {
        case SETTO:
          Jout.setIdentity();
          break;
        case ADDTO:
Justin Carpentier's avatar
Justin Carpentier committed
142
          Jout.diagonal().array() += Scalar(1);
143
144
          break;
        case RMTO:
Justin Carpentier's avatar
Justin Carpentier committed
145
          Jout.diagonal().array() -= Scalar(1);
146
147
148
149
150
          break;
        default:
          assert(false && "Wrong Op requesed value");
          break;
        }
151
152
153
    }


154
155
156
    // template <class ConfigL_t, class ConfigR_t>
    // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                       // const Eigen::MatrixBase<ConfigR_t> & q1)
Guilhem Saurel's avatar
Guilhem Saurel committed
157

158
159
160
    template <class Config_t>
    static void normalize_impl (const Eigen::MatrixBase<Config_t>& /*qout*/)
    {}
161
162

    template <class Config_t>
163
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
164
    {
gabrielebndn's avatar
gabrielebndn committed
165
      PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
166
167
168
    }

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
169
170
171
172
    void randomConfiguration_impl
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
     const Eigen::MatrixBase<ConfigOut_t> & qout) const
173
    {
gabrielebndn's avatar
gabrielebndn committed
174
      ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
175
      for (int i = 0; i < nq (); ++i)
176
      {
177
178
        if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
           upper_pos_limit[i] ==  std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
179
180
181
        {
          std::ostringstream error;
          error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
182
          // assert(false && "non bounded limit. Cannot uniformly sample joint revolute");
183
          throw std::range_error(error.str());
184
185
186
        }
        res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
      }
187
    }
188
    
189
  private:
190
    
191
192
    Eigen::internal::variable_if_dynamic<Index, Dim> size_;
  }; // struct VectorSpaceOperationTpl
193

194
} // namespace pinocchio
195

196
#endif // ifndef __pinocchio_vector_space_operation_hpp__