5 #ifndef __pinocchio_lie_group_variant_visitor_hpp__ 6 #define __pinocchio_lie_group_variant_visitor_hpp__ 8 #include "pinocchio/multibody/liegroup/fwd.hpp" 21 template<
typename LieGroupCollection>
22 inline int nq(
const LieGroupGenericTpl<LieGroupCollection> & lg);
32 template<
typename LieGroupCollection>
33 inline int nv(
const LieGroupGenericTpl<LieGroupCollection> & lg);
42 template<
typename LieGroupCollection>
43 inline std::string
name(
const LieGroupGenericTpl<LieGroupCollection> & lg);
52 template<
typename LieGroupCollection>
53 inline Eigen::Matrix<typename LieGroupCollection::Scalar,Eigen::Dynamic,1,LieGroupCollection::Options>
54 neutral(
const LieGroupGenericTpl<LieGroupCollection> & lg);
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);
70 template<
typename LieGroupCollection,
class Config_t>
71 inline void random(
const LieGroupGenericTpl<LieGroupCollection> & lg,
72 const Eigen::MatrixBase<Config_t> & qout);
74 template<
typename LieGroupCollection,
class Config_t>
75 inline void normalize(
const LieGroupGenericTpl<LieGroupCollection> & lg,
76 const Eigen::MatrixBase<Config_t> & qout);
78 template<
typename LieGroupCollection,
class ConfigL_t,
class ConfigR_t>
80 const Eigen::MatrixBase<ConfigL_t> & q0,
81 const Eigen::MatrixBase<ConfigR_t> & q1,
82 const typename ConfigL_t::Scalar& prec);
84 template<
typename LieGroupCollection,
class ConfigL_t,
class ConfigR_t>
85 inline typename ConfigL_t::Scalar
87 const Eigen::MatrixBase<ConfigL_t> & q0,
88 const Eigen::MatrixBase<ConfigR_t> & q1);
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)
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);
105 template<
typename LieGroupCollection,
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
107 const Eigen::MatrixBase<ConfigL_t> & q0,
108 const Eigen::MatrixBase<ConfigR_t> & q1,
109 const Eigen::MatrixBase<ConfigOut_t> & qout);
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);
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,
124 const AssignmentOperatorType op = SETTO);
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,
132 const Eigen::MatrixBase<JacobianOut_t> & J_out,
134 const AssignmentOperatorType op = SETTO);
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,
141 const Eigen::MatrixBase<JacobianIn_t> & J_in,
142 const Eigen::MatrixBase<JacobianOut_t> & J_out,
144 const AssignmentOperatorType op = SETTO);
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,
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,
159 const Eigen::MatrixBase<JacobianOut_t> & Jout,
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,
167 const Eigen::MatrixBase<JacobianIn_t> & Jin,
168 const Eigen::MatrixBase<JacobianOut_t> & Jout,
171 template<
typename LieGroupCollection,
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
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,
179 template<
typename LieGroupCollection,
class Config_t,
class Tangent_t,
class JacobianOut_t>
181 const Eigen::MatrixBase<Config_t > & q,
182 const Eigen::MatrixBase<Tangent_t> & v,
183 const Eigen::MatrixBase<JacobianOut_t> & J,
188 #include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx" 190 #endif // ifndef __pinocchio_lie_group_variant_visitor_hpp__ Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
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...
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...
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...
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
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.
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.
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...
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...
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.
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Main pinocchio namespace.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
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.