5 #ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
6 #define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
8 #include "pinocchio/multibody/liegroup/fwd.hpp"
15 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME) \
16 typedef LieGroupBase<Derived> Base; \
17 typedef TYPENAME Base::Index Index; \
18 typedef TYPENAME traits<Derived>::Scalar Scalar; \
20 Options = traits<Derived>::Options, \
24 typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \
25 typedef TYPENAME Base::TangentVector_t TangentVector_t; \
26 typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
28 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \
29 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)
31 #define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \
32 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
34 template<
typename Derived>
37 typedef Derived LieGroupDerived;
47 typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
48 typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
49 typedef Eigen::Matrix<Scalar,NV,NV,Options> JacobianMatrix_t;
62 template <
class ConfigIn_t,
class Tangent_t,
class ConfigOut_t>
63 void integrate(
const Eigen::MatrixBase<ConfigIn_t> & q,
64 const Eigen::MatrixBase<Tangent_t> & v,
65 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
79 template<
class Config_t,
class Jacobian_t>
81 const Eigen::MatrixBase<Jacobian_t> & J)
const;
96 template <ArgumentPosition arg,
class Config_t,
class Tangent_t,
class JacobianOut_t>
98 const Eigen::MatrixBase<Tangent_t> & v,
99 const Eigen::MatrixBase<JacobianOut_t> & J,
100 AssignmentOperatorType op = SETTO)
const
102 PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
103 return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
119 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
120 void dIntegrate(
const Eigen::MatrixBase<Config_t > & q,
121 const Eigen::MatrixBase<Tangent_t> & v,
122 const Eigen::MatrixBase<JacobianOut_t> & J,
124 const AssignmentOperatorType op = SETTO)
const;
138 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
140 const Eigen::MatrixBase<Tangent_t> & v,
141 const Eigen::MatrixBase<JacobianOut_t> & J,
142 const AssignmentOperatorType op = SETTO)
const;
156 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
158 const Eigen::MatrixBase<Tangent_t> & v,
159 const Eigen::MatrixBase<JacobianOut_t> & J,
160 const AssignmentOperatorType op = SETTO)
const;
182 template<
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
184 const Eigen::MatrixBase<Tangent_t> & v,
185 const Eigen::MatrixBase<JacobianIn_t> & Jin,
186 const Eigen::MatrixBase<JacobianOut_t> & Jout,
207 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
209 const Eigen::MatrixBase<Tangent_t> & v,
210 const Eigen::MatrixBase<JacobianIn_t> & Jin,
211 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const;
230 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
232 const Eigen::MatrixBase<Tangent_t> & v,
233 const Eigen::MatrixBase<JacobianIn_t> & Jin,
234 const Eigen::MatrixBase<JacobianOut_t> & Jout)
const;
254 template<
class Config_t,
class Tangent_t,
class Jacobian_t>
256 const Eigen::MatrixBase<Tangent_t> & v,
257 const Eigen::MatrixBase<Jacobian_t> & J,
278 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
280 const Eigen::MatrixBase<Tangent_t> & v,
281 const Eigen::MatrixBase<Jacobian_t> & J)
const;
300 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
302 const Eigen::MatrixBase<Tangent_t> & v,
303 const Eigen::MatrixBase<Jacobian_t> & J)
const;
314 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
315 void interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
316 const Eigen::MatrixBase<ConfigR_t> & q1,
318 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
326 template <
class Config_t>
327 void normalize(
const Eigen::MatrixBase<Config_t> & qout)
const;
337 template <
class Config_t>
338 void random(
const Eigen::MatrixBase<Config_t> & qout)
const;
349 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
351 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
352 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
364 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
365 void difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
366 const Eigen::MatrixBase<ConfigR_t> & q1,
367 const Eigen::MatrixBase<Tangent_t> & v)
const;
389 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
390 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
391 const Eigen::MatrixBase<ConfigR_t> & q1,
392 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
409 template<
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
410 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
411 const Eigen::MatrixBase<ConfigR_t> & q1,
412 const Eigen::MatrixBase<JacobianOut_t> & J,
422 template <
class ConfigL_t,
class ConfigR_t>
424 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
434 template <
class ConfigL_t,
class ConfigR_t>
435 Scalar
distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
436 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
446 template <
class ConfigL_t,
class ConfigR_t>
448 const Eigen::MatrixBase<ConfigR_t> & q1,
449 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
455 template <
class Config_t,
class Tangent_t>
456 ConfigVector_t
integrate(
const Eigen::MatrixBase<Config_t> & q,
457 const Eigen::MatrixBase<Tangent_t> & v)
const ;
459 template <
class ConfigL_t,
class ConfigR_t>
460 ConfigVector_t
interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
461 const Eigen::MatrixBase<ConfigR_t> & q1,
462 const Scalar& u)
const;
464 ConfigVector_t random()
const;
466 template <
class ConfigL_t,
class ConfigR_t>
468 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
469 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
const;
471 template <
class ConfigL_t,
class ConfigR_t>
472 TangentVector_t
difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
473 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
480 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
481 void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
482 const Eigen::MatrixBase<ConfigR_t> & q1,
484 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
486 template <
class Config_t>
487 void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
const;
489 template <
class ConfigL_t,
class ConfigR_t>
490 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
491 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
493 template <
class ConfigL_t,
class ConfigR_t>
494 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
495 const Eigen::MatrixBase<ConfigR_t> & q1,
496 const Scalar & prec)
const;
505 ConfigVector_t
neutral ()
const;
508 std::string
name ()
const;
512 return static_cast <Derived&
> (*this);
515 const Derived& derived ()
const
517 return static_cast <const Derived&
> (*this);
535 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
537 #endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__