5 #ifndef __pinocchio_cartesian_product_variant_hpp__ 6 #define __pinocchio_cartesian_product_variant_hpp__ 8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp" 9 #include "pinocchio/multibody/liegroup/liegroup-collection.hpp" 11 #include "pinocchio/container/aligned-vector.hpp" 16 template<
typename Scalar,
int Options = 0,
17 template<
typename,
int>
class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
21 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class LieGroupCollectionTpl>
24 typedef _Scalar Scalar;
35 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class LieGroupCollectionTpl>
37 :
public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
39 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
44 typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
50 , lg_nqs(0), lg_nvs(0)
61 , lg_nqs(0), lg_nvs(0)
74 const LieGroupGeneric & lg2)
76 , lg_nqs(0), lg_nvs(0)
79 append(lg1); append(lg2);
87 void append(
const LieGroupGeneric & lg);
116 int nq()
const {
return m_nq; }
117 int nv()
const {
return m_nv; }
119 std::string
name()
const {
return m_name; }
121 ConfigVector_t
neutral()
const {
return m_neutral; }
123 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
124 void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
125 const Eigen::MatrixBase<ConfigR_t> & q1,
126 const Eigen::MatrixBase<Tangent_t> & d)
const;
128 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
129 void dDifference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
130 const Eigen::MatrixBase<ConfigR_t> & q1,
131 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
133 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianIn_t,
class JacobianOut_t>
134 void dDifference_product_impl(
const ConfigL_t & q0,
135 const ConfigR_t & q1,
136 const JacobianIn_t & Jin,
137 JacobianOut_t & Jout,
138 bool dDifferenceOnTheLeft,
139 const AssignmentOperatorType op)
const;
141 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
142 void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
143 const Eigen::MatrixBase<Velocity_t> & v,
144 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
146 template <
class Config_t,
class Jacobian_t>
147 void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
148 const Eigen::MatrixBase<Jacobian_t> & J)
const;
150 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
151 void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
152 const Eigen::MatrixBase<Tangent_t> & v,
153 const Eigen::MatrixBase<JacobianOut_t> & J,
154 const AssignmentOperatorType op=SETTO)
const;
156 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
157 void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
158 const Eigen::MatrixBase<Tangent_t> & v,
159 const Eigen::MatrixBase<JacobianOut_t> & J,
160 const AssignmentOperatorType op=SETTO)
const;
162 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
163 void dIntegrate_product_impl(
const Config_t & q,
165 const JacobianIn_t & Jin,
166 JacobianOut_t & Jout,
167 bool dIntegrateOnTheLeft,
169 const AssignmentOperatorType op)
const;
171 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
172 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
173 const Eigen::MatrixBase<Tangent_t> & v,
174 const Eigen::MatrixBase<JacobianIn_t> & J_in,
175 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const;
177 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
178 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
179 const Eigen::MatrixBase<Tangent_t> & v,
180 const Eigen::MatrixBase<JacobianIn_t> & J_in,
181 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const;
183 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
184 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
185 const Eigen::MatrixBase<Tangent_t> & v,
186 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
188 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
189 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
190 const Eigen::MatrixBase<Tangent_t> & v,
191 const Eigen::MatrixBase<JacobianOut_t> & J)
const;
193 template <
class ConfigL_t,
class ConfigR_t>
194 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
195 const Eigen::MatrixBase<ConfigR_t> & q1)
const;
197 template <
class Config_t>
198 void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
const;
200 template <
class Config_t>
201 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const;
203 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
204 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
205 const Eigen::MatrixBase<ConfigR_t> & upper,
206 const Eigen::MatrixBase<ConfigOut_t> & qout)
const;
208 template <
class ConfigL_t,
class ConfigR_t>
209 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
210 const Eigen::MatrixBase<ConfigR_t> & q1,
211 const Scalar & prec)
const;
215 template <
typename LieGroup1,
typename LieGroup2>
220 PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
222 std::vector<Index> lg_nqs, lg_nvs;
225 ConfigVector_t m_neutral;
231 #include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx> 233 #endif // ifndef __pinocchio_cartesian_product_variant_hpp__ int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
CartesianProductOperationVariantTpl()
Default constructor.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg1, const LieGroupGeneric &lg2)
Constructor with two Lie groups.
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant.
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.
Common traits structure to fully define base classes for CRTP.
CartesianProductOperationVariantTpl(const LieGroupGeneric &lg)
Constructor with one single Lie group.
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
.