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