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__
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...
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 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.
std::string name() const
Get name of instance.
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.
Index nv() const
Get dimension of Lie Group tangent space.
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...
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
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...
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
LieGroupBase(const LieGroupBase &)
Main pinocchio namespace.
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.
Common traits structure to fully define base classes for CRTP.
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 normalize(const Eigen::MatrixBase< Config_t > &qout) const
Normalize the joint configuration given as input. For instance, the quaternion must be unitary...
ConfigVector_t neutral() const
Get neutral element as a vector.
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...