5 #ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ 6 #define __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ 8 #include <pinocchio/multibody/liegroup/liegroup-base.hpp> 12 template<
int dim1,
int dim2>
15 enum { value = dim1 + dim2 };
21 enum { value = Eigen::Dynamic };
27 enum { value = Eigen::Dynamic };
30 template<
typename LieGroup1,
typename LieGroup2>
33 template<
typename LieGroup1,
typename LieGroup2>
43 template<
typename LieGroup1,
typename LieGroup2>
57 return lg1.nq () + lg2.nq ();
63 return lg1.nv () + lg2.nv ();
70 Qo1(n) = lg1.neutral ();
71 Qo2(n) = lg2.neutral ();
75 std::string
name ()
const 77 std::ostringstream oss; oss << lg1.name () <<
"*" << lg2.name ();
81 template <
class ConfigL_t,
class ConfigR_t,
class Tangent_t>
82 void difference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
83 const Eigen::MatrixBase<ConfigR_t> & q1,
84 const Eigen::MatrixBase<Tangent_t> & d)
const 86 lg1.difference(Q1(q0), Q1(q1), Vo1(d));
87 lg2.difference(Q2(q0), Q2(q1), Vo2(d));
90 template <ArgumentPosition arg,
class ConfigL_t,
class ConfigR_t,
class JacobianOut_t>
91 void dDifference_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
92 const Eigen::MatrixBase<ConfigR_t> & q1,
93 const Eigen::MatrixBase<JacobianOut_t> & J)
const 98 lg1.template dDifference<arg> (Q1(q0), Q1(q1), J11(J));
99 lg2.template dDifference<arg> (Q2(q0), Q2(q1), J22(J));
102 template <
class ConfigIn_t,
class Velocity_t,
class ConfigOut_t>
103 void integrate_impl(
const Eigen::MatrixBase<ConfigIn_t> & q,
104 const Eigen::MatrixBase<Velocity_t> & v,
105 const Eigen::MatrixBase<ConfigOut_t> & qout)
const 107 lg1.integrate(Q1(q), V1(v), Qo1(qout));
108 lg2.integrate(Q2(q), V2(v), Qo2(qout));
111 template <
class Config_t,
class Jacobian_t>
112 void integrateCoeffWiseJacobian_impl(
const Eigen::MatrixBase<Config_t> & q,
113 const Eigen::MatrixBase<Jacobian_t> & J)
const 115 assert(J.rows() ==
nq() && J.cols() ==
nv() &&
"J is not of the right dimension");
116 Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
117 J_.topRightCorner(lg1.nq(),lg2.nv()).setZero();
118 J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero();
120 lg1.integrateCoeffWiseJacobian(Q1(q),
121 J_.topLeftCorner(lg1.nq(),lg1.nv()));
122 lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
125 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
126 void dIntegrate_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
127 const Eigen::MatrixBase<Tangent_t> & v,
128 const Eigen::MatrixBase<JacobianOut_t> & J,
129 const AssignmentOperatorType op=SETTO)
const 139 lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),op);
140 lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),op);
143 assert(
false &&
"Wrong Op requesed value");
148 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
149 void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
150 const Eigen::MatrixBase<Tangent_t> & v,
151 const Eigen::MatrixBase<JacobianOut_t> & J,
152 const AssignmentOperatorType op=SETTO)
const 162 lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),op);
163 lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),op);
166 assert(
false &&
"Wrong Op requesed value");
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 JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
178 JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
179 lg1.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
180 lg2.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
183 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
184 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
185 const Eigen::MatrixBase<Tangent_t> & v,
186 const Eigen::MatrixBase<JacobianIn_t> & J_in,
187 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const 189 JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
190 JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
191 lg1.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
192 lg2.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
196 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
197 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
198 const Eigen::MatrixBase<Tangent_t> & v,
199 const Eigen::MatrixBase<Jacobian_t> & Jin)
const 201 Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
202 lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
203 lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
206 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
207 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
208 const Eigen::MatrixBase<Tangent_t> & v,
209 const Eigen::MatrixBase<Jacobian_t> & Jin)
const 211 Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
212 lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
213 lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
216 template <
class ConfigL_t,
class ConfigR_t>
217 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
218 const Eigen::MatrixBase<ConfigR_t> & q1)
const 220 return lg1.squaredDistance(Q1(q0), Q1(q1))
221 + lg2.squaredDistance(Q2(q0), Q2(q1));
224 template <
class Config_t>
225 void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 227 lg1.normalize(Qo1(qout));
228 lg2.normalize(Qo2(qout));
231 template <
class Config_t>
232 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const 234 lg1.random(Qo1(qout));
235 lg2.random(Qo2(qout));
238 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
239 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
240 const Eigen::MatrixBase<ConfigR_t> & upper,
241 const Eigen::MatrixBase<ConfigOut_t> & qout)
244 lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
245 lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
248 template <
class ConfigL_t,
class ConfigR_t>
249 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
250 const Eigen::MatrixBase<ConfigR_t> & q1,
251 const Scalar & prec)
const 253 return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
254 && lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
259 return lg1 == other.lg1 && lg2 == other.lg2;
268 #if EIGEN_VERSION_AT_LEAST(3,2,1) 269 # define REMOVE_IF_EIGEN_TOO_LOW(x) x 271 # define REMOVE_IF_EIGEN_TOO_LOW(x) 274 template <
typename Config >
typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type Q1 (
const Eigen::MatrixBase<Config >& q)
const {
return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
275 template <
typename Config >
typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type Q2 (
const Eigen::MatrixBase<Config >& q)
const {
return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
276 template <
typename Tangent>
typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (
const Eigen::MatrixBase<Tangent>& v)
const {
return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
277 template <
typename Tangent>
typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (
const Eigen::MatrixBase<Tangent>& v)
const {
return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }
279 template <
typename Config >
typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (
const Eigen::MatrixBase<Config >& q)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); }
280 template <
typename Config >
typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (
const Eigen::MatrixBase<Config >& q)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); }
281 template <
typename Tangent>
typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (
const Eigen::MatrixBase<Tangent>& v)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); }
282 template <
typename Tangent>
typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (
const Eigen::MatrixBase<Tangent>& v)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); }
284 template <
typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (
const Eigen::MatrixBase<Jac>& J)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(),lg1.nv()); }
285 template <
typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (
const Eigen::MatrixBase<Jac>& J)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(),lg2.nv()); }
286 template <
typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (
const Eigen::MatrixBase<Jac>& J)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(),lg1.nv()); }
287 template <
typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (
const Eigen::MatrixBase<Jac>& J)
const {
return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(),lg2.nv()); }
288 #undef REMOVE_IF_EIGEN_TOO_LOW 294 #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ 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...
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
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.