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 ();
66 ConfigVector_t neutral ()
const
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
136 lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),SETTO);
137 lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),SETTO);
140 lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),ADDTO);
141 lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),ADDTO);
144 lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),RMTO);
145 lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),RMTO);
148 assert(
false &&
"Wrong Op requesed value");
153 template <
class Config_t,
class Tangent_t,
class JacobianOut_t>
154 void dIntegrate_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
155 const Eigen::MatrixBase<Tangent_t> & v,
156 const Eigen::MatrixBase<JacobianOut_t> & J,
157 const AssignmentOperatorType op=SETTO)
const
164 lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),SETTO);
165 lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),SETTO);
168 lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),ADDTO);
169 lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),ADDTO);
172 lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),RMTO);
173 lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),RMTO);
176 assert(
false &&
"Wrong Op requesed value");
181 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
182 void dIntegrateTransport_dq_impl(
const Eigen::MatrixBase<Config_t > & q,
183 const Eigen::MatrixBase<Tangent_t> & v,
184 const Eigen::MatrixBase<JacobianIn_t> & J_in,
185 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
187 JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
188 JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
189 lg1.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
190 lg2.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
193 template <
class Config_t,
class Tangent_t,
class JacobianIn_t,
class JacobianOut_t>
194 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
195 const Eigen::MatrixBase<Tangent_t> & v,
196 const Eigen::MatrixBase<JacobianIn_t> & J_in,
197 const Eigen::MatrixBase<JacobianOut_t> & J_out)
const
199 JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
200 JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
201 lg1.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
202 lg2.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
206 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
207 void dIntegrateTransport_dq_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_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
213 lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
216 template <
class Config_t,
class Tangent_t,
class Jacobian_t>
217 void dIntegrateTransport_dv_impl(
const Eigen::MatrixBase<Config_t > & q,
218 const Eigen::MatrixBase<Tangent_t> & v,
219 const Eigen::MatrixBase<Jacobian_t> & Jin)
const
221 Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
222 lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
223 lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
226 template <
class ConfigL_t,
class ConfigR_t>
227 Scalar squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
228 const Eigen::MatrixBase<ConfigR_t> & q1)
const
230 return lg1.squaredDistance(Q1(q0), Q1(q1))
231 + lg2.squaredDistance(Q2(q0), Q2(q1));
234 template <
class Config_t>
235 void normalize_impl (
const Eigen::MatrixBase<Config_t>& qout)
const
237 lg1.normalize(Qo1(qout));
238 lg2.normalize(Qo2(qout));
241 template <
class Config_t>
242 void random_impl (
const Eigen::MatrixBase<Config_t>& qout)
const
244 lg1.random(Qo1(qout));
245 lg2.random(Qo2(qout));
248 template <
class ConfigL_t,
class ConfigR_t,
class ConfigOut_t>
249 void randomConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & lower,
250 const Eigen::MatrixBase<ConfigR_t> & upper,
251 const Eigen::MatrixBase<ConfigOut_t> & qout)
254 lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
255 lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
258 template <
class ConfigL_t,
class ConfigR_t>
259 bool isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
260 const Eigen::MatrixBase<ConfigR_t> & q1,
261 const Scalar & prec)
const
263 return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
264 && lg2.isSameConfiguration(Q2(q0), Q2(q1), prec);
273 #if EIGEN_VERSION_AT_LEAST(3,2,1)
274 # define REMOVE_IF_EIGEN_TOO_LOW(x) x
276 # define REMOVE_IF_EIGEN_TOO_LOW(x)
279 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())); }
280 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())); }
281 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())); }
282 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())); }
284 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())); }
285 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())); }
286 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())); }
287 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())); }
289 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()); }
290 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()); }
291 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()); }
292 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()); }
293 #undef REMOVE_IF_EIGEN_TOO_LOW
299 #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__