pinocchio  2.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
liegroup-variant-visitors.hpp
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 ConfigL_t, class ConfigR_t>
79  inline bool isSameConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
80  const Eigen::MatrixBase<ConfigL_t> & q0,
81  const Eigen::MatrixBase<ConfigR_t> & q1,
82  const typename ConfigL_t::Scalar& prec);
83 
84  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
85  inline typename ConfigL_t::Scalar
86  squaredDistance(const LieGroupGenericTpl<LieGroupCollection> & lg,
87  const Eigen::MatrixBase<ConfigL_t> & q0,
88  const Eigen::MatrixBase<ConfigR_t> & q1);
89 
90  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
91  inline typename ConfigL_t::Scalar
92  distance(const LieGroupGenericTpl<LieGroupCollection> & lg,
93  const Eigen::MatrixBase<ConfigL_t> & q0,
94  const Eigen::MatrixBase<ConfigR_t> & q1)
95  {
96  return std::sqrt(squaredDistance(lg, q0, q1));
97  }
98 
99  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t>
100  inline void difference(const LieGroupGenericTpl<LieGroupCollection> & lg,
101  const Eigen::MatrixBase<ConfigL_t> & q0,
102  const Eigen::MatrixBase<ConfigR_t> & q1,
103  const Eigen::MatrixBase<Tangent_t> & v);
104 
105  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
106  inline void randomConfiguration(const LieGroupGenericTpl<LieGroupCollection> & lg,
107  const Eigen::MatrixBase<ConfigL_t> & q0,
108  const Eigen::MatrixBase<ConfigR_t> & q1,
109  const Eigen::MatrixBase<ConfigOut_t> & qout);
110 
111  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
112  inline void interpolate(const LieGroupGenericTpl<LieGroupCollection> & lg,
113  const Eigen::MatrixBase<ConfigL_t> & q0,
114  const Eigen::MatrixBase<ConfigR_t> & q1,
115  const typename ConfigL_t::Scalar& u,
116  const Eigen::MatrixBase<ConfigOut_t> & qout);
117 
118  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
119  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
120  const Eigen::MatrixBase<Config_t > & q,
121  const Eigen::MatrixBase<Tangent_t> & v,
122  const Eigen::MatrixBase<JacobianOut_t> & J,
123  const ArgumentPosition arg,
124  const AssignmentOperatorType op = SETTO);
125 
126  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
127  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
128  const Eigen::MatrixBase<Config_t > & q,
129  const Eigen::MatrixBase<Tangent_t> & v,
130  const Eigen::MatrixBase<JacobianIn_t> & J_in,
131  int self,
132  const Eigen::MatrixBase<JacobianOut_t> & J_out,
133  const ArgumentPosition arg,
134  const AssignmentOperatorType op = SETTO);
135 
136  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
137  void dIntegrate(const LieGroupGenericTpl<LieGroupCollection> & lg,
138  const Eigen::MatrixBase<Config_t > & q,
139  const Eigen::MatrixBase<Tangent_t> & v,
140  int self,
141  const Eigen::MatrixBase<JacobianIn_t> & J_in,
142  const Eigen::MatrixBase<JacobianOut_t> & J_out,
143  const ArgumentPosition arg,
144  const AssignmentOperatorType op = SETTO);
145 
146  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
147  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
148  const Eigen::MatrixBase<ConfigL_t> & q0,
149  const Eigen::MatrixBase<ConfigR_t> & q1,
150  const Eigen::MatrixBase<JacobianOut_t> & J,
151  const ArgumentPosition arg);
152 
153  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
154  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
155  const Eigen::MatrixBase<ConfigL_t> & q0,
156  const Eigen::MatrixBase<ConfigR_t> & q1,
157  const Eigen::MatrixBase<JacobianIn_t> & Jin,
158  int self,
159  const Eigen::MatrixBase<JacobianOut_t> & Jout,
160  const ArgumentPosition arg);
161 
162  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
163  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
164  const Eigen::MatrixBase<ConfigL_t> & q0,
165  const Eigen::MatrixBase<ConfigR_t> & q1,
166  int self,
167  const Eigen::MatrixBase<JacobianIn_t> & Jin,
168  const Eigen::MatrixBase<JacobianOut_t> & Jout,
169  const ArgumentPosition arg);
170 
171  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
172  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
173  const Eigen::MatrixBase<Config_t > & q,
174  const Eigen::MatrixBase<Tangent_t> & v,
175  const Eigen::MatrixBase<JacobianIn_t> & J_in,
176  const Eigen::MatrixBase<JacobianOut_t> & J_out,
177  const ArgumentPosition arg);
178 
179  template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
180  void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
181  const Eigen::MatrixBase<Config_t > & q,
182  const Eigen::MatrixBase<Tangent_t> & v,
183  const Eigen::MatrixBase<JacobianOut_t> & J,
184  const ArgumentPosition arg);
185 }
186 
188 #include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx"
189 
190 #endif // ifndef __pinocchio_lie_group_variant_visitor_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:579
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
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::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::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:396
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::isSameConfiguration
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec)
Return true if the given configurations are equivalents, within the given precision.
Definition: joint-configuration.hpp:648
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::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:59
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::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:505
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::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:607
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24