5 #ifndef __pinocchio_lie_group_operation_base_hpp__ 6 #define __pinocchio_lie_group_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;
95 template <ArgumentPosition arg,
class Config_t,
class Tangent_t,
class JacobianOut_t>
97 const Eigen::MatrixBase<Tangent_t> & v,
98 const Eigen::MatrixBase<JacobianOut_t> & J)
const 100 PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
101 return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
116 template<
class Config_t,
class Tangent_t,
class JacobianOut_t>
117 void dIntegrate(
const Eigen::MatrixBase<Config_t > & q,
118 const Eigen::MatrixBase<Tangent_t> & v,
119 const Eigen::MatrixBase<JacobianOut_t> & J,
133 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
135 const Eigen::MatrixBase<Tangent_t> & v,
136 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
149 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
151 const Eigen::MatrixBase<Tangent_t> & v,
152 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
163 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
164 void interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
165 const Eigen::MatrixBase<ConfigR_t> & q1,
167 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
175 template <
class Config_t>
176 void normalize(
const Eigen::MatrixBase<Config_t> & qout)
const;
186 template <
class Config_t>
187 void random(
const Eigen::MatrixBase<Config_t> & qout)
const;
198 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
200 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
201 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
213 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
214 void difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
215 const Eigen::MatrixBase<ConfigR_t> & q1,
216 const Eigen::MatrixBase<Tangent_t> & v)
const;
238 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
239 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
240 const Eigen::MatrixBase<ConfigR_t> & q1,
241 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
258 template<
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
259 void dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
260 const Eigen::MatrixBase<ConfigR_t> & q1,
261 const Eigen::MatrixBase<JacobianOut_t> & J,
271 template <
class ConfigL_t,
class ConfigR_t>
273 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
283 template <
class ConfigL_t,
class ConfigR_t>
284 Scalar
distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
285 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
295 template <
class ConfigL_t,
class ConfigR_t>
297 const Eigen::MatrixBase<ConfigR_t> & q1,
298 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const;
304 template <
class Config_t,
class Tangent_t>
305 ConfigVector_t
integrate(
const Eigen::MatrixBase<Config_t> & q,
306 const Eigen::MatrixBase<Tangent_t> & v)
const ;
308 template <
class ConfigL_t,
class ConfigR_t>
309 ConfigVector_t
interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
310 const Eigen::MatrixBase<ConfigR_t> & q1,
311 const Scalar& u)
const;
313 ConfigVector_t random()
const;
315 template <
class ConfigL_t,
class ConfigR_t>
317 (
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
318 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
const;
320 template <
class ConfigL_t,
class ConfigR_t>
321 TangentVector_t
difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
322 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
329 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
330 void interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
331 const Eigen::MatrixBase<ConfigR_t> & q1,
333 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
335 template <
class Config_t>
336 void normalize_impl(
const Eigen::MatrixBase<Config_t> & qout)
const;
338 template <
class ConfigL_t,
class ConfigR_t>
339 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
340 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
342 template <
class ConfigL_t,
class ConfigR_t>
343 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
344 const Eigen::MatrixBase<ConfigR_t> & q1,
345 const Scalar & prec)
const;
354 ConfigVector_t
neutral ()
const;
357 std::string
name ()
const;
361 return static_cast <Derived&> (*this);
364 const Derived& derived ()
const 366 return static_cast <
const Derived&> (*this);
384 #include "pinocchio/multibody/liegroup/liegroup-base.hxx" 386 #endif // ifndef __pinocchio_lie_group_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 dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
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 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 a small variation of the tangent vector into tangent space at identity...
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 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 a small variation of the configuration vector into tangent space at identity...
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.
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 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.