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__
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.
void normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
void integrateCoeffWiseJacobian(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J) const
Computes the Jacobian of the integrate operator around zero.
void dIntegrateTransport_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
ConfigVector_t neutral() const
Get neutral element as a vector.
void integrate(const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout) const
Integrate a joint's configuration with a tangent vector during one unit time duration.
std::string name() const
Get name of instance.
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...
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
void dIntegrateTransport(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout, const ArgumentPosition arg) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
void dIntegrate_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the tangent vector into tangent space at identity...
void dIntegrate_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector into tangent space at identity...
void dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
LieGroupBase(const LieGroupBase &)
Main pinocchio namespace.
Index nv() const
Get dimension of Lie Group tangent space.
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Common traits structure to fully define base classes for CRTP.
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.
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's configurations.
void dIntegrateTransport_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianIn_t > &Jin, const Eigen::MatrixBase< JacobianOut_t > &Jout) const
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
void dDifference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of the difference operation with respect to q0 or q1.