pinocchio  3.4.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
liegroup-base.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
6 #define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
7 
8 #include "pinocchio/multibody/liegroup/fwd.hpp"
9 
10 #include <limits>
11 
12 namespace pinocchio
13 {
14  constexpr int SELF = 0;
15 
16 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, TYPENAME) \
17  typedef LieGroupBase<Derived> Base; \
18  typedef TYPENAME Base::Index Index; \
19  typedef TYPENAME traits<Derived>::Scalar Scalar; \
20  enum \
21  { \
22  Options = traits<Derived>::Options, \
23  NQ = Base::NQ, \
24  NV = Base::NV \
25  }; \
26  typedef TYPENAME Base::ConfigVector_t ConfigVector_t; \
27  typedef TYPENAME Base::TangentVector_t TangentVector_t; \
28  typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
29 
30 #define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived) \
31  PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, PINOCCHIO_MACRO_EMPTY_ARG)
32 
33 #define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived) \
34  PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived, typename)
35 
36  template<typename Derived>
37  struct LieGroupBase
38  {
39  typedef Derived LieGroupDerived;
40  typedef int Index;
41  typedef typename traits<LieGroupDerived>::Scalar Scalar;
42  enum
43  {
44  Options = traits<LieGroupDerived>::Options,
45  NQ = traits<LieGroupDerived>::NQ,
46  NV = traits<LieGroupDerived>::NV
47  };
48 
49  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
50  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
51  typedef Eigen::Matrix<Scalar, NV, NV, Options> JacobianMatrix_t;
52 
55 
65  template<class ConfigIn_t, class Tangent_t, class ConfigOut_t>
66  void integrate(
67  const Eigen::MatrixBase<ConfigIn_t> & q,
68  const Eigen::MatrixBase<Tangent_t> & v,
69  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
70 
85  template<class Config_t, class Jacobian_t>
86  void integrateCoeffWiseJacobian(
87  const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const;
88 
104  template<ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
105  void dIntegrate(
106  const Eigen::MatrixBase<Config_t> & q,
107  const Eigen::MatrixBase<Tangent_t> & v,
108  const Eigen::MatrixBase<JacobianOut_t> & J,
109  AssignmentOperatorType op = SETTO) const
110  {
111  PINOCCHIO_STATIC_ASSERT(arg == ARG0 || arg == ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
112  return dIntegrate(
113  q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), arg, op);
114  }
115 
131  template<class Config_t, class Tangent_t, class JacobianOut_t>
132  void dIntegrate(
133  const Eigen::MatrixBase<Config_t> & q,
134  const Eigen::MatrixBase<Tangent_t> & v,
135  const Eigen::MatrixBase<JacobianOut_t> & J,
136  const ArgumentPosition arg,
137  const AssignmentOperatorType op = SETTO) const;
138 
152  template<class Config_t, class Tangent_t, class JacobianOut_t>
153  void dIntegrate_dq(
154  const Eigen::MatrixBase<Config_t> & q,
155  const Eigen::MatrixBase<Tangent_t> & v,
156  const Eigen::MatrixBase<JacobianOut_t> & J,
157  const AssignmentOperatorType op = SETTO) const;
158 
159  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
160  void dIntegrate_dq(
161  const Eigen::MatrixBase<Config_t> & q,
162  const Eigen::MatrixBase<Tangent_t> & v,
163  const Eigen::MatrixBase<JacobianIn_t> & Jin,
164  int self,
165  const Eigen::MatrixBase<JacobianOut_t> & Jout,
166  const AssignmentOperatorType op = SETTO) const;
167 
168  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
169  void dIntegrate_dq(
170  const Eigen::MatrixBase<Config_t> & q,
171  const Eigen::MatrixBase<Tangent_t> & v,
172  int self,
173  const Eigen::MatrixBase<JacobianIn_t> & Jin,
174  const Eigen::MatrixBase<JacobianOut_t> & Jout,
175  const AssignmentOperatorType op = SETTO) const;
176 
190  template<class Config_t, class Tangent_t, class JacobianOut_t>
191  void dIntegrate_dv(
192  const Eigen::MatrixBase<Config_t> & q,
193  const Eigen::MatrixBase<Tangent_t> & v,
194  const Eigen::MatrixBase<JacobianOut_t> & J,
195  const AssignmentOperatorType op = SETTO) const;
196 
197  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
198  void dIntegrate_dv(
199  const Eigen::MatrixBase<Config_t> & q,
200  const Eigen::MatrixBase<Tangent_t> & v,
201  int self,
202  const Eigen::MatrixBase<JacobianIn_t> & Jin,
203  const Eigen::MatrixBase<JacobianOut_t> & Jout,
204  const AssignmentOperatorType op = SETTO) const;
205 
206  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
207  void dIntegrate_dv(
208  const Eigen::MatrixBase<Config_t> & q,
209  const Eigen::MatrixBase<Tangent_t> & v,
210  const Eigen::MatrixBase<JacobianIn_t> & Jin,
211  int self,
212  const Eigen::MatrixBase<JacobianOut_t> & Jout,
213  const AssignmentOperatorType op = SETTO) const;
214 
240  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
241  void dIntegrateTransport(
242  const Eigen::MatrixBase<Config_t> & q,
243  const Eigen::MatrixBase<Tangent_t> & v,
244  const Eigen::MatrixBase<JacobianIn_t> & Jin,
245  const Eigen::MatrixBase<JacobianOut_t> & Jout,
246  const ArgumentPosition arg) const;
247 
270  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
271  void dIntegrateTransport_dq(
272  const Eigen::MatrixBase<Config_t> & q,
273  const Eigen::MatrixBase<Tangent_t> & v,
274  const Eigen::MatrixBase<JacobianIn_t> & Jin,
275  const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
298  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
299  void dIntegrateTransport_dv(
300  const Eigen::MatrixBase<Config_t> & q,
301  const Eigen::MatrixBase<Tangent_t> & v,
302  const Eigen::MatrixBase<JacobianIn_t> & Jin,
303  const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
304 
327  template<class Config_t, class Tangent_t, class Jacobian_t>
328  void dIntegrateTransport(
329  const Eigen::MatrixBase<Config_t> & q,
330  const Eigen::MatrixBase<Tangent_t> & v,
331  const Eigen::MatrixBase<Jacobian_t> & J,
332  const ArgumentPosition arg) const;
333 
355  template<class Config_t, class Tangent_t, class Jacobian_t>
356  void dIntegrateTransport_dq(
357  const Eigen::MatrixBase<Config_t> & q,
358  const Eigen::MatrixBase<Tangent_t> & v,
359  const Eigen::MatrixBase<Jacobian_t> & J) const;
381  template<class Config_t, class Tangent_t, class Jacobian_t>
382  void dIntegrateTransport_dv(
383  const Eigen::MatrixBase<Config_t> & q,
384  const Eigen::MatrixBase<Tangent_t> & v,
385  const Eigen::MatrixBase<Jacobian_t> & J) const;
386 
396  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
397  void interpolate(
398  const Eigen::MatrixBase<ConfigL_t> & q0,
399  const Eigen::MatrixBase<ConfigR_t> & q1,
400  const Scalar & u,
401  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
402 
413  template<class Config_t>
414  void normalize(const Eigen::MatrixBase<Config_t> & qout) const;
415 
424  template<class Config_t>
425  bool isNormalized(
426  const Eigen::MatrixBase<Config_t> & qin,
427  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
428 
437  template<class Config_t>
438  void random(const Eigen::MatrixBase<Config_t> & qout) const;
439 
449  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
450  void randomConfiguration(
451  const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
452  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
453  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
454 
470  template<class ConfigL_t, class ConfigR_t, class Tangent_t>
471  void difference(
472  const Eigen::MatrixBase<ConfigL_t> & q0,
473  const Eigen::MatrixBase<ConfigR_t> & q1,
474  const Eigen::MatrixBase<Tangent_t> & v) const;
475 
497  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
498  void dDifference(
499  const Eigen::MatrixBase<ConfigL_t> & q0,
500  const Eigen::MatrixBase<ConfigR_t> & q1,
501  const Eigen::MatrixBase<JacobianOut_t> & J) const;
502 
514  template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
515  void dDifference(
516  const Eigen::MatrixBase<ConfigL_t> & q0,
517  const Eigen::MatrixBase<ConfigR_t> & q1,
518  const Eigen::MatrixBase<JacobianOut_t> & J,
519  const ArgumentPosition arg) const;
520 
521  template<
522  ArgumentPosition arg,
523  class ConfigL_t,
524  class ConfigR_t,
525  class JacobianIn_t,
526  class JacobianOut_t>
527  void dDifference(
528  const Eigen::MatrixBase<ConfigL_t> & q0,
529  const Eigen::MatrixBase<ConfigR_t> & q1,
530  const Eigen::MatrixBase<JacobianIn_t> & Jin,
531  int self,
532  const Eigen::MatrixBase<JacobianOut_t> & Jout,
533  const AssignmentOperatorType op = SETTO) const;
534 
535  template<
536  ArgumentPosition arg,
537  class ConfigL_t,
538  class ConfigR_t,
539  class JacobianIn_t,
540  class JacobianOut_t>
541  void dDifference(
542  const Eigen::MatrixBase<ConfigL_t> & q0,
543  const Eigen::MatrixBase<ConfigR_t> & q1,
544  int self,
545  const Eigen::MatrixBase<JacobianIn_t> & Jin,
546  const Eigen::MatrixBase<JacobianOut_t> & Jout,
547  const AssignmentOperatorType op = SETTO) const;
548 
557  template<class ConfigL_t, class ConfigR_t>
558  Scalar squaredDistance(
559  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
560 
569  template<class ConfigL_t, class ConfigR_t>
570  Scalar distance(
571  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
572 
582  template<class ConfigL_t, class ConfigR_t>
583  bool isSameConfiguration(
584  const Eigen::MatrixBase<ConfigL_t> & q0,
585  const Eigen::MatrixBase<ConfigR_t> & q1,
586  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
587 
588  bool operator==(const LieGroupBase & other) const
589  {
590  return derived().isEqual_impl(other.derived());
591  }
592 
593  bool operator!=(const LieGroupBase & other) const
594  {
595  return derived().isDifferent_impl(other.derived());
596  }
598 
601 
602  template<class Config_t, class Tangent_t>
603  ConfigVector_t
604  integrate(const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Tangent_t> & v) const;
605 
606  template<class ConfigL_t, class ConfigR_t>
607  ConfigVector_t interpolate(
608  const Eigen::MatrixBase<ConfigL_t> & q0,
609  const Eigen::MatrixBase<ConfigR_t> & q1,
610  const Scalar & u) const;
611 
612  ConfigVector_t random() const;
613 
614  template<class ConfigL_t, class ConfigR_t>
615  ConfigVector_t randomConfiguration(
616  const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
617  const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
618 
619  template<class ConfigL_t, class ConfigR_t>
620  TangentVector_t difference(
621  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
623 
626 
627  template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
628  void dIntegrate_product_impl(
629  const Config_t & q,
630  const Tangent_t & v,
631  const JacobianIn_t & Jin,
632  JacobianOut_t & Jout,
633  bool dIntegrateOnTheLeft,
634  const ArgumentPosition arg,
635  const AssignmentOperatorType op) const;
636 
637  template<
638  ArgumentPosition arg,
639  class ConfigL_t,
640  class ConfigR_t,
641  class JacobianIn_t,
642  class JacobianOut_t>
643  void dDifference_product_impl(
644  const ConfigL_t & q0,
645  const ConfigR_t & q1,
646  const JacobianIn_t & Jin,
647  JacobianOut_t & Jout,
648  bool dDifferenceOnTheLeft,
649  const AssignmentOperatorType op) const;
650 
651  template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
652  void interpolate_impl(
653  const Eigen::MatrixBase<ConfigL_t> & q0,
654  const Eigen::MatrixBase<ConfigR_t> & q1,
655  const Scalar & u,
656  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
657 
658  template<class Config_t>
659  void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
660 
661  template<class Config_t>
662  bool isNormalized_impl(
663  const Eigen::MatrixBase<Config_t> & qin,
664  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
665 
666  template<class ConfigL_t, class ConfigR_t>
667  Scalar squaredDistance_impl(
668  const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const;
669 
670  template<class ConfigL_t, class ConfigR_t>
671  bool isSameConfiguration_impl(
672  const Eigen::MatrixBase<ConfigL_t> & q0,
673  const Eigen::MatrixBase<ConfigR_t> & q1,
674  const Scalar & prec) const;
675 
678  bool isEqual_impl(const LieGroupBase & /*other*/) const
679  {
680  return true;
681  }
682  bool isDifferent_impl(const LieGroupBase & other) const
683  {
684  return !derived().isEqual_impl(other.derived());
685  }
686 
691  Index nq() const;
693  Index nv() const;
695  ConfigVector_t neutral() const;
696 
698  std::string name() const;
699 
700  Derived & derived()
701  {
702  return static_cast<Derived &>(*this);
703  }
704 
705  const Derived & derived() const
706  {
707  return static_cast<const Derived &>(*this);
708  }
710 
711  protected:
715  LieGroupBase()
716  {
717  }
718 
722  LieGroupBase(const LieGroupBase & /*clone*/)
723  {
724  }
725 
729  LieGroupBase & operator=(const LieGroupBase & /*other*/)
730  {
731  return *this;
732  }
733 
734  // C++11
735  // LieGroupBase(const LieGroupBase &) = delete;
736  // LieGroupBase& operator=(const LieGroupBase & /*x*/) = delete;
737  }; // struct LieGroupBase
738 
739 } // namespace pinocchio
740 
741 #include "pinocchio/multibody/liegroup/liegroup-base.hxx"
742 
743 #endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:122
pinocchio::LieGroupBase::random
void random(const Eigen::MatrixBase< Config_t > &qout) const
Generate a random joint configuration, normalizing quaternions when necessary.
pinocchio::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's configurations.
pinocchio::LieGroupBase::name
std::string name() const
Get name of instance.
pinocchio::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's configuration with a tangent vector during one unit time duration.
pinocchio::LieGroupBase::nv
Index nv() const
Get dimension of Lie Group tangent space.
pinocchio::LieGroupBase::dIntegrateTransport
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 initial tangent space of the integrate operation,...
pinocchio::LieGroupBase::dIntegrateTransport_dq
void dIntegrateTransport_dq(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J) const
Transport in place a matrix from the terminal to the initial tangent space of the integrate operation...
pinocchio::LieGroupBase::isEqual_impl
bool isEqual_impl(const LieGroupBase &) const
Default equality check. By default, two LieGroupBase of same type are considered equal.
Definition: liegroup-base.hpp:678
pinocchio::LieGroupBase::dIntegrateTransport_dv
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 initial tangent space of the integrate operation,...
pinocchio::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.
pinocchio::LieGroupBase::LieGroupBase
LieGroupBase(const LieGroupBase &)
Definition: liegroup-base.hpp:722
pinocchio::LieGroupBase::dIntegrateTransport_dq
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 initial tangent space of the integrate operation,...
pinocchio::LieGroupBase::distance
Scalar distance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Distance between two configurations of the joint.
pinocchio::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 AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector into tangent space at identity...
pinocchio::LieGroupBase::squaredDistance
Scalar squaredDistance(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1) const
Squared distance between two joint configurations.
pinocchio::LieGroupBase::dIntegrateTransport
void dIntegrateTransport(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J, const ArgumentPosition arg) const
Transport in place a matrix from the terminal to the initial tangent space of the integrate operation...
pinocchio::LieGroupBase::operator=
LieGroupBase & operator=(const LieGroupBase &)
Definition: liegroup-base.hpp:729
pinocchio::LieGroupBase::dDifference
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.
pinocchio::LieGroupBase::dDifference
void dDifference(const Eigen::MatrixBase< ConfigL_t > &q0, const Eigen::MatrixBase< ConfigR_t > &q1, const Eigen::MatrixBase< JacobianOut_t > &J, const ArgumentPosition arg) const
Computes the Jacobian of the difference operation with respect to q0 or q1.
pinocchio::LieGroupBase::dIntegrateTransport_dv
void dIntegrateTransport_dv(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< Jacobian_t > &J) const
Transport in place a matrix from the terminal to the initial tangent space of the integrate operation...
pinocchio::LieGroupBase::neutral
ConfigVector_t neutral() const
Get neutral element as a vector.
pinocchio::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 AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the tangent vector into tangent space at identity.
pinocchio::LieGroupBase::dIntegrate
void dIntegrate(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< JacobianOut_t > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO) const
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tan...
pinocchio::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.
pinocchio::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.
pinocchio::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.
pinocchio::LieGroupBase::dIntegrate
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...
Definition: liegroup-base.hpp:105
pinocchio::LieGroupBase::isNormalized
bool isNormalized(const Eigen::MatrixBase< Config_t > &qin, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Check whether the input joint configuration is normalized. For instance, the quaternion must be unita...
pinocchio::LieGroupBase::integrateCoeffWiseJacobian
void integrateCoeffWiseJacobian(const Eigen::MatrixBase< Config_t > &q, const Eigen::MatrixBase< Jacobian_t > &J) const
Computes the Jacobian of the integrate operator around zero.
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72