multibody/liegroup/cartesian-prod: dintegratetransport

src/multibody/liegroup/cartesian-product.hpp

@@ -181,17 +181,25 @@ namespace pinocchio
     template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
     void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
                                      const Eigen::MatrixBase<Tangent_t> & v,
-                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
-                                     const Eigen::MatrixBase<JacobianOut_t> & Jout) const
+                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,
+                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const
     {
+      JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
+      JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
+      lg1_.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
+      lg2_.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
     }
 
     template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
     void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
                                      const Eigen::MatrixBase<Tangent_t> & v,
-                                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
-                                     const Eigen::MatrixBase<JacobianOut_t> & Jout) const
+                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,
+                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const
     {
+      JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
+      JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
+      lg1_.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
+      lg2_.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
     }
     
     template <class ConfigL_t, class ConfigR_t>