pinocchio  UNKNOWN
operation-base.hxx
1 //
2 // Copyright (c) 2016-2017 CNRS
3 //
4 // This file is part of Pinocchio
5 // Pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // Pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // Pinocchio If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef __se3_lie_group_operation_base_hxx__
19 #define __se3_lie_group_operation_base_hxx__
20 
21 #include "pinocchio/macros.hpp"
22 
23 namespace se3 {
24 
25  // --------------- API with return value as argument ---------------------- //
26 
27  template <class Derived>
28  template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
29  void LieGroupBase<Derived>::integrate(
30  const Eigen::MatrixBase<ConfigIn_t> & q,
31  const Eigen::MatrixBase<Tangent_t> & v,
32  const Eigen::MatrixBase<ConfigOut_t>& qout) const
33  {
34  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t);
35  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
36  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
37  derived().integrate_impl(q, v, qout);
38  }
39 
40  template <class Derived>
41  template <class Config_t, class Tangent_t, class JacobianOut_t>
42  void LieGroupBase<Derived>::dIntegrate_dq(
43  const Eigen::MatrixBase<Config_t > & q,
44  const Eigen::MatrixBase<Tangent_t> & v,
45  const Eigen::MatrixBase<JacobianOut_t>& J) const
46  {
47  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
48  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
49  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
50  derived().dIntegrate_dq_impl(q, v, J);
51  }
52 
53  template <class Derived>
54  template <class Config_t, class Tangent_t, class JacobianOut_t>
55  void LieGroupBase<Derived>::dIntegrate_dv(
56  const Eigen::MatrixBase<Config_t > & q,
57  const Eigen::MatrixBase<Tangent_t> & v,
58  const Eigen::MatrixBase<JacobianOut_t>& J) const
59  {
60  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
61  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
62  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
63  derived().dIntegrate_dv_impl(q, v, J);
64  }
65 
75  template <class Derived>
76  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
77  void LieGroupBase<Derived>::interpolate(
78  const Eigen::MatrixBase<ConfigL_t> & q0,
79  const Eigen::MatrixBase<ConfigR_t> & q1,
80  const Scalar& u,
81  const Eigen::MatrixBase<ConfigOut_t>& qout) const
82  {
83  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
84  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
85  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
86  Derived().interpolate_impl(q0, q1, u, qout);
87  }
88 
89  template <class Derived>
90  template <class Config_t>
91  void LieGroupBase<Derived>::normalize
92  (const Eigen::MatrixBase<Config_t>& qout) const
93  {
94  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
95  return derived().normalize_impl (qout);
96  }
97 
106  template <class Derived>
107  template <class Config_t>
108  void LieGroupBase<Derived>::random
109  (const Eigen::MatrixBase<Config_t>& qout) const
110  {
111  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
112  return derived().random_impl (qout);
113  }
114 
115  template <class Derived>
116  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
117  void LieGroupBase<Derived>::randomConfiguration(
118  const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
119  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
120  const Eigen::MatrixBase<ConfigOut_t> & qout) const
121  {
122  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
123  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
124  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
125  derived ().randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
126  }
127 
128  template <class Derived>
129  template <class ConfigL_t, class ConfigR_t, class Tangent_t>
130  void LieGroupBase<Derived>::difference(
131  const Eigen::MatrixBase<ConfigL_t> & q0,
132  const Eigen::MatrixBase<ConfigR_t> & q1,
133  const Eigen::MatrixBase<Tangent_t>& d) const
134  {
135  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
136  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
137  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
138  derived().difference_impl(q0, q1, d);
139  }
140 
141  template <class Derived>
142  template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
143  void LieGroupBase<Derived>::Jdifference(
144  const Eigen::MatrixBase<ConfigL_t> & q0,
145  const Eigen::MatrixBase<ConfigR_t> & q1,
146  const Eigen::MatrixBase<JacobianLOut_t>& J0,
147  const Eigen::MatrixBase<JacobianROut_t>& J1) const
148  {
149  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
150  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
151  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianLOut_t, JacobianMatrix_t);
152  EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianROut_t, JacobianMatrix_t);
153  derived().Jdifference_impl (q0, q1, J0, J1);
154  }
155 
156  template <class Derived>
157  template <class ConfigL_t, class ConfigR_t>
158  typename LieGroupBase<Derived>::Scalar
159  LieGroupBase<Derived>::squaredDistance(
160  const Eigen::MatrixBase<ConfigL_t> & q0,
161  const Eigen::MatrixBase<ConfigR_t> & q1) const
162  {
163  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
164  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
165  return derived().squaredDistance_impl(q0, q1);
166  }
167 
168  template <class Derived>
169  template <class ConfigL_t, class ConfigR_t>
170  typename LieGroupBase<Derived>::Scalar
171  LieGroupBase<Derived>::distance(
172  const Eigen::MatrixBase<ConfigL_t> & q0,
173  const Eigen::MatrixBase<ConfigR_t> & q1) const
174  {
175  return sqrt(squaredDistance(q0, q1));
176  }
177 
178  template <class Derived>
179  template <class ConfigL_t, class ConfigR_t>
180  bool LieGroupBase<Derived>::isSameConfiguration(
181  const Eigen::MatrixBase<ConfigL_t> & q0,
182  const Eigen::MatrixBase<ConfigR_t> & q1,
183  const Scalar & prec) const
184  {
185  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
186  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
187  return derived().isSameConfiguration_impl(q0, q1, prec);
188  }
189 
190  // ----------------- API that allocates memory ---------------------------- //
191 
192 
193  template <class Derived>
194  template <class Config_t, class Tangent_t>
195  typename LieGroupBase<Derived>::ConfigVector_t
196  LieGroupBase<Derived>::integrate(const Eigen::MatrixBase<Config_t> & q,
197  const Eigen::MatrixBase<Tangent_t> & v) const
198  {
199  ConfigVector_t qout;
200  integrate(q, v, qout);
201  return qout;
202  }
203 
204  template <class Derived>
205  template <class ConfigL_t, class ConfigR_t>
206  typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::interpolate(
207  const Eigen::MatrixBase<ConfigL_t> & q0,
208  const Eigen::MatrixBase<ConfigR_t> & q1,
209  const Scalar& u) const
210  {
211  ConfigVector_t qout;
212  interpolate(q0, q1, u, qout);
213  return qout;
214  }
215 
216  template <class Derived>
217  typename LieGroupBase<Derived>::ConfigVector_t
218  LieGroupBase<Derived>::random() const
219  {
220  ConfigVector_t qout;
221  random(qout);
222  return qout;
223  }
224 
225  template <class Derived>
226  template <class ConfigL_t, class ConfigR_t>
227  typename LieGroupBase<Derived>::ConfigVector_t
228  LieGroupBase<Derived>::randomConfiguration
229  (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
230  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const
231  {
232  ConfigVector_t qout;
233  randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
234  return qout;
235  }
236 
237  template <class Derived>
238  template <class ConfigL_t, class ConfigR_t>
239  typename LieGroupBase<Derived>::TangentVector_t LieGroupBase<Derived>::difference(
240  const Eigen::MatrixBase<ConfigL_t> & q0,
241  const Eigen::MatrixBase<ConfigR_t> & q1) const
242  {
243  TangentVector_t diff;
244  difference(q0, q1, diff);
245  return diff;
246  }
247 
248  // ----------------- Default implementations ------------------------------ //
249  template <class Derived>
250  template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
251  void LieGroupBase<Derived>::interpolate_impl(
252  const Eigen::MatrixBase<ConfigL_t> & q0,
253  const Eigen::MatrixBase<ConfigR_t> & q1,
254  const Scalar& u,
255  const Eigen::MatrixBase<ConfigOut_t>& qout) const
256  {
257  if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
258  else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
259  else
260  {
261  TangentVector_t vdiff(u * difference(q0, q1));
262  integrate(q0, vdiff, qout);
263  }
264  }
265 
266  template <class Derived>
267  template <class ConfigL_t, class ConfigR_t>
268  typename LieGroupBase<Derived>::Scalar
269  LieGroupBase<Derived>::squaredDistance_impl(
270  const Eigen::MatrixBase<ConfigL_t> & q0,
271  const Eigen::MatrixBase<ConfigR_t> & q1) const
272  {
273  TangentVector_t t;
274  difference(q0, q1, t);
275  return t.squaredNorm();
276  }
277 
278  template <class Derived>
279  template <class ConfigL_t, class ConfigR_t>
280  bool LieGroupBase<Derived>::isSameConfiguration_impl(
281  const Eigen::MatrixBase<ConfigL_t> & q0,
282  const Eigen::MatrixBase<ConfigR_t> & q1,
283  const Scalar & prec) const
284  {
285  return q0.isApprox(q1, prec);
286  }
287 
288  template <class Derived>
289  typename LieGroupBase <Derived>::Index
290  LieGroupBase <Derived>::nq () const
291  {
292  return derived().nq();
293  }
294 
295  template <class Derived>
296  typename LieGroupBase <Derived>::Index
297  LieGroupBase <Derived>::nv () const
298  {
299  return derived().nv();
300  }
301 
302  template <class Derived>
303  typename LieGroupBase <Derived>::ConfigVector_t
304  LieGroupBase <Derived>::neutral () const
305  {
306  return derived().neutral();
307  }
308 
309  template <class Derived>
310  std::string LieGroupBase <Derived>::name () const
311  {
312  return derived().name();
313  }
314 
315 } // namespace se3
316 
317 #endif // __se3_lie_group_operation_base_hxx__
se3::squaredDistance
Eigen::VectorXd squaredDistance(const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
Squared distance between two configuration vectors.
Definition: joint-configuration.hxx:106
se3::LieGroupBase::distance
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Definition: operation-base.hxx:171
se3::LieGroupBase::normalize
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
Definition: operation-base.hxx:92
se3::LieGroupBase::integrate
void integrate(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Integrate a joint&#39;s configuration with a tangent vector during one unit time duration.
Definition: operation-base.hxx:29
se3::difference
Eigen::VectorXd difference(const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1...
Definition: joint-configuration.hxx:83
se3::LieGroupBase::name
std::string name() const
Get name of instance.
Definition: operation-base.hxx:310
se3::LieGroupBase::nq
Index nq() const
Definition: operation-base.hxx:290
se3::LieGroupBase::interpolate
void interpolate(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &u, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Interpolation between two joint&#39;s configurations.
Definition: operation-base.hxx:77
se3::LieGroupBase::dIntegrate_dq
void dIntegrate_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of the Integrate operation.
Definition: operation-base.hxx:42
se3::interpolate
Eigen::VectorXd interpolate(const Model &model, const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, const double u)
Interpolate the model between two configurations.
Definition: joint-configuration.hxx:57
se3::LieGroupBase::squaredDistance
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
Definition: operation-base.hxx:159
se3::LieGroupBase::isSameConfiguration
bool isSameConfiguration(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Check if two configurations are equivalent within the given precision.
Definition: operation-base.hxx:180
se3::integrate
Eigen::VectorXd integrate(const Model &model, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Integrate a configuration for the specified model for a tangent vector during one unit time...
Definition: joint-configuration.hxx:34
se3::LieGroupBase::randomConfiguration
void randomConfiguration(const Eigen::MatrixBase< ConfigL_t > &lower_pos_limit, const Eigen::MatrixBase< ConfigR_t > &upper_pos_limit, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Generate a configuration vector uniformly sampled among provided limits.
Definition: operation-base.hxx:117
se3::randomConfiguration
Eigen::VectorXd randomConfiguration(const Model &model, const Eigen::VectorXd &lowerLimits, const Eigen::VectorXd &upperLimits)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hxx:146
se3::LieGroupBase::nv
Index nv() const
Get dimension of Lie Group tangent space.
Definition: operation-base.hxx:297
se3::LieGroupBase::neutral
ConfigVector_t neutral() const
Get neutral element as a vector.
Definition: operation-base.hxx:304
se3::LieGroupBase::dIntegrate_dv
void dIntegrate_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of the Integrate operation.
Definition: operation-base.hxx:55
se3::LieGroupBase::Jdifference
void Jdifference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianLOut_t > &J0, const Eigen::MatrixBase< JacobianROut_t > &J1) const
Computes the Jacobian of the difference operation.
Definition: operation-base.hxx:143
se3::LieGroupBase::difference
void difference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< Tangent_t > &v) const
Computes the tangent vector that must be integrated during one unit time to go from q0 to q1...
Definition: operation-base.hxx:130