[LieGroup] Add LieGroup dDifference with matrix product.

bindings/python/multibody/liegroups.hpp

@@ -46,7 +46,7 @@ struct LieGroupWrapperTpl
     return lg.difference(q0,q1);
   }
 
-  static JacobianMatrix_t dDifference(const LieGroupType& lg,
+  static JacobianMatrix_t dDifference1(const LieGroupType& lg,
       const ConfigVector_t& q0, const ConfigVector_t& q1,
       const ArgumentPosition arg)
   {
@@ -55,6 +55,44 @@ struct LieGroupWrapperTpl
     return J;
   }
 
+  static JacobianMatrix_t dDifference2(const LieGroupType& lg,
+      const ConfigVector_t& q0, const ConfigVector_t& q1,
+      const ArgumentPosition arg,
+      const JacobianMatrix_t& Jin, int self)
+  {
+    JacobianMatrix_t J (Jin.rows(), Jin.cols());
+    switch (arg) {
+      case ARG0:
+        lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
+        break;
+      case ARG1:
+        lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
+        break;
+      default:
+        throw std::invalid_argument("arg must be either ARG0 or ARG1");
+    }
+    return J;
+  }
+
+  static JacobianMatrix_t dDifference3(const LieGroupType& lg,
+      const ConfigVector_t& q0, const ConfigVector_t& q1,
+      const ArgumentPosition arg,
+      int self, const JacobianMatrix_t& Jin)
+  {
+    JacobianMatrix_t J (Jin.rows(), Jin.cols());
+    switch (arg) {
+      case ARG0:
+        lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
+        break;
+      case ARG1:
+        lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
+        break;
+      default:
+        throw std::invalid_argument("arg must be either ARG0 or ARG1");
+    }
+    return J;
+  }
+
   static JacobianMatrix_t dIntegrate_dq1(const LieGroupType& lg,
       const ConfigVector_t& q, const TangentVector_t& v)
   {
@@ -152,7 +190,9 @@ public:
     .def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport2)
 
     .def("difference", LieGroupWrapper::difference)
-    .def("dDifference", LieGroupWrapper::dDifference)
+    .def("dDifference", LieGroupWrapper::dDifference1)
+    .def("dDifference", LieGroupWrapper::dDifference2)
+    .def("dDifference", LieGroupWrapper::dDifference3)
 
     .def("interpolate", LieGroupWrapper::interpolate)
 

src/multibody/liegroup/cartesian-product-variant.hpp

@@ -110,6 +110,14 @@ namespace pinocchio
                           const Eigen::MatrixBase<ConfigR_t> & q1,
                           const Eigen::MatrixBase<JacobianOut_t> & J) const;
 
+    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+    void dDifference_product_impl(const ConfigL_t & q0,
+                                  const ConfigR_t & q1,
+                                  const JacobianIn_t & Jin,
+                                  JacobianOut_t & Jout,
+                                  bool dDifferenceOnTheLeft,
+                                  const AssignmentOperatorType op) const;
+
     template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
     void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                         const Eigen::MatrixBase<Velocity_t> & v,

src/multibody/liegroup/cartesian-product-variant.hxx

@@ -71,6 +71,42 @@ dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
   }
 }
 
+template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
+template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
+dDifference_product_impl(const ConfigL_t & q0,
+                         const ConfigR_t & q1,
+                         const JacobianIn_t & Jin,
+                         JacobianOut_t & Jout,
+                         bool dDifferenceOnTheLeft,
+                         const AssignmentOperatorType op) const
+{
+  Index id_q = 0, id_v = 0;
+  for(size_t k = 0; k < liegroups.size(); ++k)
+  {
+    const Index & nq = lg_nqs[k];
+    const Index & nv = lg_nvs[k];
+    if (dDifferenceOnTheLeft)
+      ::pinocchio::dDifference<arg>(liegroups[k],
+                       q0.segment(id_q,nq),
+                       q1.segment(id_q,nq),
+                       SELF,
+                       Jin.middleRows(id_v,nv),
+                       Jout.middleRows(id_v,nv),
+                       op);
+    else
+      ::pinocchio::dDifference<arg>(liegroups[k],
+                       q0.segment(id_q,nq),
+                       q1.segment(id_q,nq),
+                       Jin.middleCols(id_v,nv),
+                       SELF,
+                       Jout.middleCols(id_v,nv),
+                       op);
+
+    id_q += nq; id_v += nv;
+  }
+}
+
 template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
 template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
 void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::

src/multibody/liegroup/liegroup-base.hpp

@@ -427,10 +427,6 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
     void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                      const Eigen::MatrixBase<ConfigR_t> & q1,
                      const Eigen::MatrixBase<JacobianOut_t> & J) const;
-//    {
-//      PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
-//      return dDifference(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
-//    }
 
     /**
      *
@@ -448,6 +444,23 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
                      const Eigen::MatrixBase<ConfigR_t> & q1,
                      const Eigen::MatrixBase<JacobianOut_t> & J,
                      const ArgumentPosition arg) const;
+
+    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                     const Eigen::MatrixBase<ConfigR_t> & q1,
+                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                     int self,
+                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                     const AssignmentOperatorType op = SETTO) const;
+
+    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                     const Eigen::MatrixBase<ConfigR_t> & q1,
+                     int self,
+                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                     const AssignmentOperatorType op = SETTO) const;
+
     /**
      * @brief      Squared distance between two joint configurations.
      *
@@ -533,6 +546,14 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
                                  const ArgumentPosition arg,
                                  const AssignmentOperatorType op) const;
 
+    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+    void dDifference_product_impl(const ConfigL_t & q0,
+                                  const ConfigR_t & q1,
+                                  const JacobianIn_t & Jin,
+                                  JacobianOut_t & Jout,
+                                  bool dDifferenceOnTheLeft,
+                                  const AssignmentOperatorType op) const;
+
     template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
     void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                           const Eigen::MatrixBase<ConfigR_t> & q1,

src/multibody/liegroup/liegroup-base.hxx

@@ -401,6 +401,48 @@ namespace pinocchio {
     }
   }
 
+  template <class Derived>
+  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                          const Eigen::MatrixBase<ConfigR_t> & q1,
+                                          const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                                          int self,
+                                          const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                                          const AssignmentOperatorType op) const
+  {
+    PINOCCHIO_UNUSED_VARIABLE(self);
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
+    assert(Jin.cols() == nv());
+    assert(Jout.cols() == nv());
+    assert(Jout.rows() == Jin.rows());
+    derived().template dDifference_product_impl<arg>(
+        q0.derived(), q1.derived(),
+        Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
+        false, op);
+  }
+
+  template <class Derived>
+  template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
+                                          const Eigen::MatrixBase<ConfigR_t> & q1,
+                                          int self,
+                                          const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                                          const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                                          const AssignmentOperatorType op) const
+  {
+    PINOCCHIO_UNUSED_VARIABLE(self);
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
+    EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
+    assert(Jin.rows() == nv());
+    assert(Jout.rows() == nv());
+    assert(Jout.cols() == Jin.cols());
+    derived().template dDifference_product_impl<arg>(
+        q0.derived(), q1.derived(),
+        Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
+        true, op);
+  }
+
   template <class Derived>
   template <class ConfigL_t, class ConfigR_t>
   typename LieGroupBase<Derived>::Scalar
@@ -524,6 +566,34 @@ namespace pinocchio {
     }
   }
 
+  template <class Derived>
+  template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void LieGroupBase<Derived>::dDifference_product_impl(const ConfigL_t & q0,
+                                                       const ConfigR_t & q1,
+                                                       const JacobianIn_t & Jin,
+                                                       JacobianOut_t & Jout,
+                                                       bool dDifferenceOnTheLeft,
+                                                       const AssignmentOperatorType op) const
+  {
+    Index nv_ (nv());
+    JacobianMatrix_t J (nv_, nv_);
+    dDifference<arg>(q0, q1, J);
+    switch (op) {
+      case SETTO:
+        if(dDifferenceOnTheLeft) Jout = J * Jin;
+        else                     Jout = Jin * J;
+        return;
+      case ADDTO:
+        if(dDifferenceOnTheLeft) Jout += J * Jin;
+        else                     Jout += Jin * J;
+        return;
+      case RMTO:
+        if(dDifferenceOnTheLeft) Jout -= J * Jin;
+        else                     Jout -= Jin * J;
+        return;
+    }
+  }
+
   template <class Derived>
   template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
   void LieGroupBase<Derived>::interpolate_impl(

src/multibody/liegroup/liegroup-variant-visitors.hpp

@@ -150,6 +150,24 @@ namespace pinocchio
                    const Eigen::MatrixBase<JacobianOut_t> & J,
                    const ArgumentPosition arg);
 
+  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
+                   const Eigen::MatrixBase<ConfigL_t> & q0,
+                   const Eigen::MatrixBase<ConfigR_t> & q1,
+                   const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                   int self,
+                   const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                   const ArgumentPosition arg);
+
+  template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
+                   const Eigen::MatrixBase<ConfigL_t> & q0,
+                   const Eigen::MatrixBase<ConfigR_t> & q1,
+                   int self,
+                   const Eigen::MatrixBase<JacobianIn_t> & Jin,
+                   const Eigen::MatrixBase<JacobianOut_t> & Jout,
+                   const ArgumentPosition arg);
+
   template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
   void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
                            const Eigen::MatrixBase<Config_t > & q,

src/multibody/liegroup/liegroup-variant-visitors.hxx

@@ -508,6 +508,69 @@ namespace pinocchio
           PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
   }
 
+  template <class M1_t, class M2_t, class M3_t, class M4_t, bool dDifferenceOnTheLeft, ArgumentPosition arg>
+  struct LieGroupDDifferenceProductVisitor
+  : visitor::LieGroupVisitorBase< LieGroupDDifferenceProductVisitor<M1_t, M2_t, M3_t, M4_t, dDifferenceOnTheLeft, arg> >
+  {
+    typedef boost::fusion::vector<const M1_t &,
+                                  const M2_t &,
+                                  const M3_t &,
+                                  M4_t &,
+                                  const AssignmentOperatorType> ArgsType;
+
+    LIE_GROUP_VISITOR(LieGroupDDifferenceProductVisitor);
+
+    template<typename LieGroupDerived>
+    static void algo(const LieGroupBase<LieGroupDerived> & lg,
+                     const M1_t& q0,
+                     const M2_t& q1,
+                     const M3_t& J_in,
+                     M4_t& J_out,
+                     const AssignmentOperatorType op)
+    {
+      if(dDifferenceOnTheLeft) lg.template dDifference<arg>(q0, q1, SELF, J_in, J_out, op);
+      else                     lg.template dDifference<arg>(q0, q1, J_in, SELF, J_out, op);
+    }
+  };
+
+  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
+                   const Eigen::MatrixBase<ConfigL_t> & q0,
+                   const Eigen::MatrixBase<ConfigR_t> & q1,
+                   const Eigen::MatrixBase<JacobianIn_t> & J_in,
+                   int self,
+                   const Eigen::MatrixBase<JacobianOut_t> & J_out,
+                   const AssignmentOperatorType op)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(self);
+    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
+    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
+
+    typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, false, arg> Operation;
+    Operation::run(lg,typename Operation::ArgsType(
+          q0.derived(),q1.derived(),J_in.derived(),
+          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
+  }
+
+  template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+  void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
+                   const Eigen::MatrixBase<ConfigL_t> & q0,
+                   const Eigen::MatrixBase<ConfigR_t> & q1,
+                   int self,
+                   const Eigen::MatrixBase<JacobianIn_t> & J_in,
+                   const Eigen::MatrixBase<JacobianOut_t> & J_out,
+                   const AssignmentOperatorType op)
+  {
+    PINOCCHIO_UNUSED_VARIABLE(self);
+    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
+    PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
+
+    typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, true, arg> Operation;
+    Operation::run(lg,typename Operation::ArgsType(
+          q0.derived(),q1.derived(),J_in.derived(),
+          PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
+  }
+
   template <class M1_t, class M2_t, class M3_t, class M4_t>
   struct LieGroupDIntegrateTransportVisitor
   : visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t> >

src/multibody/liegroup/vector-space.hpp

@@ -87,6 +87,30 @@ namespace pinocchio
         PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
     }
 
+    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
+    void dDifference_product_impl(const ConfigL_t &,
+                                  const ConfigR_t &,
+                                  const JacobianIn_t & Jin,
+                                  JacobianOut_t & Jout,
+                                  bool,
+                                  const AssignmentOperatorType op) const
+    {
+      switch (op) {
+        case SETTO:
+          if (arg == ARG0) Jout = - Jin;
+          else             Jout =   Jin;
+          return;
+        case ADDTO:
+          if (arg == ARG0) Jout -= Jin;
+          else             Jout += Jin;
+          return;
+        case RMTO:
+          if (arg == ARG0) Jout += Jin;
+          else             Jout -= Jin;
+          return;
+      }
+    }
+
     template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
     static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
                                const Eigen::MatrixBase<Velocity_t> & v,

unittest/python/bindings_liegroups.py

@@ -20,6 +20,8 @@ class TestLiegroupBindings(unittest.TestCase):
         self.assertEqual(R3.nv+SO3.nv, R3xSO3.nv)
 
     def test_dIntegrate(self):
+        SELF = 0
+
         for lg in [ pin.liegroups.R3(),
                     pin.liegroups.SO3(),
                     pin.liegroups.SE3(),
@@ -29,16 +31,46 @@ class TestLiegroupBindings(unittest.TestCase):
             v = np.random.rand(lg.nv)
 
             J = lg.dIntegrate_dq(q, v)
-            #self.assertTrue(np.allclose(np.eye(3),J))
 
-            SELF = 0
             J0 = np.random.rand(lg.nv,lg.nv)
 
             J1 = lg.dIntegrate_dq(q, v, SELF, J0)
-            self.assertTrue(np.allclose(J @ J0,J1))
+            self.assertTrue(np.allclose(np.dot(J, J0),J1))
 
             J1 = lg.dIntegrate_dq(q, v, J0, SELF)
-            self.assertTrue(np.allclose(J0 @ J,J1))
+            self.assertTrue(np.allclose(np.dot(J0, J),J1))
+
+            J = lg.dIntegrate_dv(q, v)
+
+            J0 = np.random.rand(lg.nv,lg.nv)
+
+            J1 = lg.dIntegrate_dv(q, v, SELF, J0)
+            self.assertTrue(np.allclose(np.dot(J, J0),J1))
+
+            J1 = lg.dIntegrate_dv(q, v, J0, SELF)
+            self.assertTrue(np.allclose(np.dot(J0, J),J1))
+
+    def test_dDifference(self):
+        for lg in [ pin.liegroups.R3(),
+                    pin.liegroups.SO3(),
+                    pin.liegroups.SE3(),
+                    pin.liegroups.R3() * pin.liegroups.SO3(),
+                    ]:
+            q0 = lg.random()
+            q1 = lg.random()
+
+            for arg in [ pin.ARG0, pin.ARG1 ]:
+                J = lg.dDifference(q0, q1, arg)
+
+                SELF = 0
+                J0 = np.random.rand(lg.nv,lg.nv)
+
+
+                J1 = lg.dDifference(q0, q1, arg, SELF, J0)
+                self.assertTrue(np.allclose(np.dot(J, J0),J1))
+
+                J1 = lg.dDifference(q0, q1, arg, J0, SELF)
+                self.assertTrue(np.allclose(np.dot(J0, J),J1))
 
 if __name__ == '__main__':
     unittest.main()