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)
76 , lg_nqs(0), lg_nvs(0)
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>
216 bool isEqual(
const CartesianProductOperation<LieGroup1, LieGroup2> & other)
const;
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__