liegroup-variant-visitors.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018 CNRS
3 //
4 
5 #ifndef __pinocchio_lie_group_variant_visitor_hpp__
6 #define __pinocchio_lie_group_variant_visitor_hpp__
7 
8 #include "pinocchio/multibody/liegroup/fwd.hpp"
9 
10 namespace pinocchio
11 {
12 
21  template<typename LieGroupCollection>
22  inline int nq(const LieGroupGenericTpl<LieGroupCollection> & lg);
23 
32  template<typename LieGroupCollection>
33  inline int nv(const LieGroupGenericTpl<LieGroupCollection> & lg);
34 
42  template<typename LieGroupCollection>
43  inline std::string name(const LieGroupGenericTpl<LieGroupCollection> & lg);
44 
52  template<typename LieGroupCollection>
53  inline Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options>
54  neutral(const LieGroupGenericTpl<LieGroupCollection> & lg);
55 
64  template<typename LieGroupCollection, class ConfigIn_t, class Tangent_t, class ConfigOut_t>
65  inline void integrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
66  const Eigen::MatrixBase<ConfigIn_t> & q,
67  const Eigen::MatrixBase<Tangent_t> & v,
68  const Eigen::MatrixBase<ConfigOut_t>& qout);
69 
70  template<typename LieGroupCollection, class Config_t>
71  inline void random(const LieGroupGenericTpl<LieGroupCollection> & lg,
72  const Eigen::MatrixBase<Config_t> & qout);
73 
74  template<typename LieGroupCollection, class Config_t>
75  inline void normalize(const LieGroupGenericTpl<LieGroupCollection> & lg,
76  const Eigen::MatrixBase<Config_t> & qout);
77 
78  template<typename LieGroupCollection, class Config_t>
79  inline bool isNormalized(const LieGroupGenericTpl<LieGroupCollection> & lg,
80  const Eigen::MatrixBase<Config_t> & qin,
81  const typename Config_t::Scalar& prec = Eigen::NumTraits<typename Config_t::Scalar>::dummy_precision());
82 
83  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
84  inline bool isSameConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
85  const Eigen::MatrixBase<ConfigL_t> & q0,
86  const Eigen::MatrixBase<ConfigR_t> & q1,
87  const typename ConfigL_t::Scalar& prec);
88 
89  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
90  inline typename ConfigL_t::Scalar
91  squaredDistance(const LieGroupGenericTpl<LieGroupCollection> & lg,
92  const Eigen::MatrixBase<ConfigL_t> & q0,
93  const Eigen::MatrixBase<ConfigR_t> & q1);
94 
95  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
96  inline typename ConfigL_t::Scalar
97  distance(const LieGroupGenericTpl<LieGroupCollection> & lg,
98  const Eigen::MatrixBase<ConfigL_t> & q0,
99  const Eigen::MatrixBase<ConfigR_t> & q1)
100  {
101  return std::sqrt(squaredDistance(lg, q0, q1));
102  }
103 
104  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t>
105  inline void difference(const LieGroupGenericTpl<LieGroupCollection> & lg,
106  const Eigen::MatrixBase<ConfigL_t> & q0,
107  const Eigen::MatrixBase<ConfigR_t> & q1,
108  const Eigen::MatrixBase<Tangent_t> & v);
109 
110  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
111  inline void randomConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
112  const Eigen::MatrixBase<ConfigL_t> & q0,
113  const Eigen::MatrixBase<ConfigR_t> & q1,
114  const Eigen::MatrixBase<ConfigOut_t> & qout);
115 
116  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
117  inline void interpolate(const LieGroupGenericTpl<LieGroupCollection> & lg,
118  const Eigen::MatrixBase<ConfigL_t> & q0,
119  const Eigen::MatrixBase<ConfigR_t> & q1,
120  const typename ConfigL_t::Scalar& u,
121  const Eigen::MatrixBase<ConfigOut_t> & qout);
122 
123  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
124  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
125  const Eigen::MatrixBase<Config_t > & q,
126  const Eigen::MatrixBase<Tangent_t> & v,
127  const Eigen::MatrixBase<JacobianOut_t> & J,
128  const ArgumentPosition arg,
129  const AssignmentOperatorType op = SETTO);
130 
131  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
132  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
133  const Eigen::MatrixBase<Config_t > & q,
134  const Eigen::MatrixBase<Tangent_t> & v,
135  const Eigen::MatrixBase<JacobianIn_t> & J_in,
136  int self,
137  const Eigen::MatrixBase<JacobianOut_t> & J_out,
138  const ArgumentPosition arg,
139  const AssignmentOperatorType op = SETTO);
140 
141  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
142  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
143  const Eigen::MatrixBase<Config_t > & q,
144  const Eigen::MatrixBase<Tangent_t> & v,
145  int self,
146  const Eigen::MatrixBase<JacobianIn_t> & J_in,
147  const Eigen::MatrixBase<JacobianOut_t> & J_out,
148  const ArgumentPosition arg,
149  const AssignmentOperatorType op = SETTO);
150 
151  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
152  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
153  const Eigen::MatrixBase<ConfigL_t> & q0,
154  const Eigen::MatrixBase<ConfigR_t> & q1,
155  const Eigen::MatrixBase<JacobianOut_t> & J,
156  const ArgumentPosition arg);
157 
158  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
159  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
160  const Eigen::MatrixBase<ConfigL_t> & q0,
161  const Eigen::MatrixBase<ConfigR_t> & q1,
162  const Eigen::MatrixBase<JacobianIn_t> & Jin,
163  int self,
164  const Eigen::MatrixBase<JacobianOut_t> & Jout);
165 
166  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
167  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
168  const Eigen::MatrixBase<ConfigL_t> & q0,
169  const Eigen::MatrixBase<ConfigR_t> & q1,
170  int self,
171  const Eigen::MatrixBase<JacobianIn_t> & Jin,
172  const Eigen::MatrixBase<JacobianOut_t> & Jout);
173 
174  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
175  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
176  const Eigen::MatrixBase<Config_t > & q,
177  const Eigen::MatrixBase<Tangent_t> & v,
178  const Eigen::MatrixBase<JacobianIn_t> & J_in,
179  const Eigen::MatrixBase<JacobianOut_t> & J_out,
180  const ArgumentPosition arg);
181 
182  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
183  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
184  const Eigen::MatrixBase<Config_t > & q,
185  const Eigen::MatrixBase<Tangent_t> & v,
186  const Eigen::MatrixBase<JacobianOut_t> & J,
187  const ArgumentPosition arg);
188 }
189 
191 #include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx"
192 
193 #endif // ifndef __pinocchio_lie_group_variant_visitor_hpp__
pinocchio::interpolate
void interpolate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u, const Eigen::MatrixBase< ReturnType > &qout)
Interpolate two configurations for a given model.
Definition: joint-configuration.hpp:96
pinocchio::LieGroupGenericTpl
Definition: multibody/liegroup/fwd.hpp:12
fwd.hpp
pinocchio::distance
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
Definition: joint-configuration.hpp:581
pinocchio::SETTO
@ SETTO
Definition: fwd.hpp:70
pinocchio::u
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
Definition: joint-configuration.hpp:787
pinocchio::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::dIntegrate
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
Definition: joint-configuration.hpp:337
inverse-kinematics.J
J
Definition: inverse-kinematics.py:31
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
pinocchio::difference
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
Definition: joint-configuration.hpp:142
pinocchio::dIntegrateTransport
void dIntegrateTransport(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType1 > &Jin, const Eigen::MatrixBase< JacobianMatrixType2 > &Jout, const ArgumentPosition arg)
Transport a matrix from the terminal to the originate tangent space of the integrate operation,...
Definition: joint-configuration.hpp:398
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
pinocchio::isNormalized
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.
Definition: joint-configuration.hpp:641
pinocchio::random
void random(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< Config_t > &qout)
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:15
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:54
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:785
pinocchio::AssignmentOperatorType
AssignmentOperatorType
Definition: fwd.hpp:68
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:747
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:748
pinocchio::squaredDistance
void squaredDistance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &out)
Squared distance between two configuration vectors.
Definition: joint-configuration.hpp:179
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:786
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
Definition: joint-configuration.hpp:507
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::isSameConfiguration
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Return true if the given configurations are equivalents, within the given precision.
Definition: joint-configuration.hpp:683
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28

pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:59