liegroup/special-euclidean: transport in place

src/multibody/liegroup/special-euclidean.hpp

@@ -385,6 +385,44 @@ namespace pinocchio
 
     }
 
+    template <class Config_t, class Tangent_t, class Jacobian_t>
+    void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
+                                     const Eigen::MatrixBase<Tangent_t> & v,
+                                     const Eigen::MatrixBase<Jacobian_t> & J) const
+    {
+      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
+      Matrix2 R;
+      Vector2 t;
+      exp(v, R, t);
+
+      Vector2 tinv = (R.transpose() * t).reverse();
+      tinv[0] *= Scalar(-1.);
+      //TODO: Aliasing
+      Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
+      //No Aliasing
+      Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
+    }
+
+    template <class Config_t, class Tangent_t, class Jacobian_t>
+    void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
+                                     const Eigen::MatrixBase<Tangent_t> & v,
+                                     const Eigen::MatrixBase<Jacobian_t> & J) const
+    {
+      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
+      MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; 
+
+      Eigen::Matrix<Scalar,6,6> Jtmp6;
+      Jexp6(nu, Jtmp6);
+      //TODO: Remove aliasing
+      Jout.template topRows<2>() = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
+      Jout.template topRows<2>().noalias() += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
+      Jout.template bottomRows<1>().noalias() = Jtmp6.template bottomLeftCorner<1,2>()* Jout.template topRows<2>();
+      Jout.template bottomRows<1>() += Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
+    }
+
+
+
+
     // interpolate_impl use default implementation.
     // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
     // static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
@@ -687,6 +725,37 @@ namespace pinocchio
       Jout.template bottomRows<3>().noalias() = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
     }
 
+
+    template <class Config_t, class Tangent_t, class Jacobian_t>
+    void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
+                                     const Eigen::MatrixBase<Tangent_t> & v,
+                                     const Eigen::MatrixBase<Jacobian_t> & J_out) const
+    {
+      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
+      Eigen::Matrix<Scalar,6,6> Jtmp6;
+      Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
+
+      //Aliasing
+      Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
+      Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
+      Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
+    }
+
+    template <class Config_t, class Tangent_t, class Jacobian_t>
+    void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
+                                     const Eigen::MatrixBase<Tangent_t> & v,
+                                     const Eigen::MatrixBase<Jacobian_t> & J_out) const
+    {
+      Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
+      Eigen::Matrix<Scalar,6,6> Jtmp6;
+      Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
+
+      Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
+      Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
+      Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
+    }
+
+    
     
     // interpolate_impl use default implementation.
     // template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>