pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
vector-space.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
6 #define __pinocchio_multibody_liegroup_vector_space_operation_hpp__
7 
8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9 
10 #include <stdexcept>
11 #include <boost/integer/static_min_max.hpp>
12 
13 namespace pinocchio
14 {
15  template<int Dim, typename Scalar, int Options = 0> struct VectorSpaceOperationTpl;
16 
17  template<int Dim, typename _Scalar, int _Options>
18  struct traits< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
19  {
20  typedef _Scalar Scalar;
21  enum {
22  Options = _Options,
23  NQ = Dim,
24  NV = Dim
25  };
26  };
27 
28  template<int Dim, typename _Scalar, int _Options>
29  struct VectorSpaceOperationTpl
30  : public LieGroupBase< VectorSpaceOperationTpl<Dim,_Scalar,_Options> >
31  {
32  PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);
33 
37  VectorSpaceOperationTpl(int size = boost::static_signed_max<0,Dim>::value)
38  : size_(size)
39  {
40  assert(size_.value() >= 0);
41  }
42 
45  VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
46  : Base(), size_(other.size_.value())
47  {
48  assert(size_.value() >= 0);
49  }
50 
51  VectorSpaceOperationTpl& operator=(const VectorSpaceOperationTpl& other)
52  {
53  size_.setValue(other.size_.value());
54  assert(size_.value() >= 0);
55  return *this;
56  }
57 
58  Index nq () const
59  {
60  return size_.value();
61  }
62  Index nv () const
63  {
64  return size_.value();
65  }
66 
67  ConfigVector_t neutral () const
68  {
69  return ConfigVector_t::Zero(size_.value());
70  }
71 
72  std::string name () const
73  {
74  std::ostringstream oss; oss << "R^" << nq();
75  return oss.str ();
76  }
77 
78  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
79  static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
80  const Eigen::MatrixBase<ConfigR_t> & q1,
81  const Eigen::MatrixBase<Tangent_t> & d)
82  {
83  PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d) = q1 - q0;
84  }
85 
86  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
87  void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> &,
88  const Eigen::MatrixBase<ConfigR_t> &,
89  const Eigen::MatrixBase<JacobianOut_t> & J) const
90  {
91  if (arg == ARG0)
92  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J) = -JacobianMatrix_t::Identity(size_.value(),size_.value());
93  else if (arg == ARG1)
94  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
95  }
96 
97  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
98  static void dDifference_product_impl(const ConfigL_t &,
99  const ConfigR_t &,
100  const JacobianIn_t & Jin,
101  JacobianOut_t & Jout,
102  bool,
103  const AssignmentOperatorType op)
104  {
105  switch (op) {
106  case SETTO:
107  if (arg == ARG0) Jout = - Jin;
108  else Jout = Jin;
109  return;
110  case ADDTO:
111  if (arg == ARG0) Jout -= Jin;
112  else Jout += Jin;
113  return;
114  case RMTO:
115  if (arg == ARG0) Jout += Jin;
116  else Jout -= Jin;
117  return;
118  }
119  }
120 
121  template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
122  static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
123  const Eigen::MatrixBase<Velocity_t> & v,
124  const Eigen::MatrixBase<ConfigOut_t> & qout)
125  {
126  PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q + v;
127  }
128 
129  template <class Config_t, class Jacobian_t>
130  static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> &,
131  const Eigen::MatrixBase<Jacobian_t> & J)
132  {
133  PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).setIdentity();
134  }
135 
136  template <class Config_t, class Tangent_t, class JacobianOut_t>
137  static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
138  const Eigen::MatrixBase<Tangent_t> & /*v*/,
139  const Eigen::MatrixBase<JacobianOut_t> & J,
140  const AssignmentOperatorType op=SETTO)
141  {
142  Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
143  switch(op)
144  {
145  case SETTO:
146  Jout.setIdentity();
147  break;
148  case ADDTO:
149  Jout.diagonal().array() += Scalar(1);
150  break;
151  case RMTO:
152  Jout.diagonal().array() -= Scalar(1);
153  break;
154  default:
155  assert(false && "Wrong Op requesed value");
156  break;
157  }
158  }
159 
160  template <class Config_t, class Tangent_t, class JacobianOut_t>
161  static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
162  const Eigen::MatrixBase<Tangent_t> & /*v*/,
163  const Eigen::MatrixBase<JacobianOut_t> & J,
164  const AssignmentOperatorType op=SETTO)
165  {
166  Eigen::MatrixBase<JacobianOut_t>& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
167  switch(op)
168  {
169  case SETTO:
170  Jout.setIdentity();
171  break;
172  case ADDTO:
173  Jout.diagonal().array() += Scalar(1);
174  break;
175  case RMTO:
176  Jout.diagonal().array() -= Scalar(1);
177  break;
178  default:
179  assert(false && "Wrong Op requesed value");
180  break;
181  }
182  }
183 
184  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
185  static void dIntegrate_product_impl(const Config_t &,
186  const Tangent_t &,
187  const JacobianIn_t & Jin,
188  JacobianOut_t & Jout,
189  bool,
190  const ArgumentPosition,
191  const AssignmentOperatorType op)
192  {
193  switch(op) {
194  case SETTO:
195  Jout = Jin;
196  break;
197  case ADDTO:
198  Jout += Jin;
199  break;
200  case RMTO:
201  Jout -= Jin;
202  break;
203  default:
204  assert(false && "Wrong Op requesed value");
205  break;
206  }
207  }
208 
209  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
210  static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
211  const Eigen::MatrixBase<Tangent_t> & /*v*/,
212  const Eigen::MatrixBase<JacobianIn_t> & Jin,
213  const Eigen::MatrixBase<JacobianOut_t> & Jout)
214  {
215  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
216  }
217 
218  template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
219  static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
220  const Eigen::MatrixBase<Tangent_t> & /*v*/,
221  const Eigen::MatrixBase<JacobianIn_t> & Jin,
222  const Eigen::MatrixBase<JacobianOut_t> & Jout)
223  {
224  PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
225  }
226 
227  template <class Config_t, class Tangent_t, class Jacobian_t>
228  static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
229  const Eigen::MatrixBase<Tangent_t> & /*v*/,
230  const Eigen::MatrixBase<Jacobian_t> & /*J*/) {}
231 
232  template <class Config_t, class Tangent_t, class Jacobian_t>
233  static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
234  const Eigen::MatrixBase<Tangent_t> & /*v*/,
235  const Eigen::MatrixBase<Jacobian_t> & /*J*/) {}
236 
237 
238 
239  // template <class ConfigL_t, class ConfigR_t>
240  // static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
241  // const Eigen::MatrixBase<ConfigR_t> & q1)
242 
243  template <class Config_t>
244  static void normalize_impl (const Eigen::MatrixBase<Config_t>& /*qout*/)
245  {}
246 
247  template <class Config_t>
248  static bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& /*qout*/, const Scalar& /*prec*/)
249  {
250  return true;
251  }
252 
253  template <class Config_t>
254  static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
255  {
256  PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom();
257  }
258 
259  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
260  void randomConfiguration_impl
261  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
262  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
263  const Eigen::MatrixBase<ConfigOut_t> & qout) const
264  {
265  ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).derived();
266  for (int i = 0; i < nq (); ++i)
267  {
268  if(lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity() ||
269  upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity() )
270  {
271  std::ostringstream error;
272  error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
273  // assert(false && "non bounded limit. Cannot uniformly sample joint revolute");
274  throw std::range_error(error.str());
275  }
276  res[i] = lower_pos_limit[i] + (( upper_pos_limit[i] - lower_pos_limit[i]) * rand())/RAND_MAX;
277  }
278  }
279 
280  bool isEqual_impl (const VectorSpaceOperationTpl& other) const
281  {
282  return size_.value() == other.size_.value();
283  }
284 
285  private:
286 
287  Eigen::internal::variable_if_dynamic<Index, Dim> size_;
288  }; // struct VectorSpaceOperationTpl
289 
290 } // namespace pinocchio
291 
292 #endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
pinocchio::VectorSpaceOperationTpl::VectorSpaceOperationTpl
VectorSpaceOperationTpl(const VectorSpaceOperationTpl &other)
Definition: vector-space.hpp:45
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
pinocchio::LieGroupBase
Definition: liegroup-base.hpp:40
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::VectorSpaceOperationTpl
Definition: vector-space.hpp:15
pinocchio::VectorSpaceOperationTpl::VectorSpaceOperationTpl
VectorSpaceOperationTpl(int size=boost::static_signed_max< 0, Dim >::value)
Definition: vector-space.hpp:37
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11