Merge pull request #591 from jcarpent/devel

Add dIntegrate for model + bug fixes

src/algorithm/joint-configuration.hpp

 //
-// Copyright (c) 2016-2018 CNRS
+// Copyright (c) 2016-2018 CNRS INRIA
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -37,6 +37,24 @@ namespace pinocchio
   integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
             const Eigen::MatrixBase<ConfigVectorType> & q,
             const Eigen::MatrixBase<TangentVectorType> & v);
+  
+  /**
+   * @brief      Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
+   *
+   * @param[in]  model   Model that must be integrated
+   * @param[in]  q       Initial configuration (size model.nq)
+   * @param[in]  v       Velocity (size model.nv)
+   * @param[out] J       Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
+   * @param[in]  arg     Argument (either q or v) with respect to which the
+   *
+   */
+  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
+  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                  const Eigen::MatrixBase<ConfigVectorType> & q,
+                  const Eigen::MatrixBase<TangentVectorType> & v,
+                  const Eigen::MatrixBase<JacobianMatrixType> & J,
+                  const ArgumentPosition arg);
+  
   /**
    * @brief      Interpolate the model between two configurations
    *

src/algorithm/joint-configuration.hxx

 //
-// Copyright (c) 2016-2018 CNRS
+// Copyright (c) 2016-2018 CNRS INRIA
 //
 // This file is part of Pinocchio
 // Pinocchio is free software: you can redistribute it
@@ -58,6 +58,34 @@ namespace pinocchio
     }
     return res;
   }
+  
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
+  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                  const Eigen::MatrixBase<ConfigVectorType> & q,
+                  const Eigen::MatrixBase<TangentVectorType> & v,
+                  const Eigen::MatrixBase<JacobianMatrixType> & J,
+                  const ArgumentPosition arg)
+  {
+    return dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), J.derived(),arg);
+  }
+  
+  template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
+  void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                  const Eigen::MatrixBase<ConfigVectorType> & q,
+                  const Eigen::MatrixBase<TangentVectorType> & v,
+                  const Eigen::MatrixBase<JacobianMatrixType> & J,
+                  const ArgumentPosition arg)
+  {
+    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef typename Model::JointIndex JointIndex;
+    
+    typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
+    typename Algo::ArgsType args(q.derived(),v.derived(),EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
+    for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
+    {
+      Algo::run(model.joints[i], args);
+    }
+  }
 
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
   inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)

src/macros.hpp

@@ -49,6 +49,9 @@ namespace pinocchio
 /// \brief Empty macro argument
 #define PINOCCHIO_MACRO_EMPTY_ARG
 
+/// \brief Helper to declare that a parameter is unused
+#define PINOCCHIO_UNUSED_VARIABLE(var) (void)(var)
+
 /// Ensure that a matrix (or vector) is of correct size (compile-time and run-time assertion)
 #define PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(type,M,nrows,ncols)              \
   EIGEN_STATIC_ASSERT(   (type::RowsAtCompileTime == Eigen::Dynamic || type::RowsAtCompileTime == nrows) \

src/multibody/joint/joint-base.hpp

 //
-// Copyright (c) 2015-2016,2018 CNRS
+// Copyright (c) 2015-2018 CNRS INRIA
 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 //
 // This file is part of Pinocchio
@@ -154,18 +154,113 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
       typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
       typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
     };
+    
+    template<typename D>
+    static typename SegmentReturn<D>::ConstType
+    segment(const Eigen::MatrixBase<D> & mat,
+            typename Eigen::DenseBase<D>::Index start,
+            typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template segment<NV>(start);
+    }
+    
+    template<typename D>
+    static typename SegmentReturn<D>::Type
+    segment(Eigen::MatrixBase<D> & mat,
+            typename Eigen::DenseBase<D>::Index start,
+            typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template segment<NV>(start);
+    }
+    
     template<class Mat>
     struct ColsReturn
     {
       typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
       typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
     };
+    
+    template<typename D>
+    static typename ColsReturn<D>::ConstType
+    middleCols(const Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template middleCols<NV>(start);
+    }
+    
+    template<typename D>
+    static typename ColsReturn<D>::Type
+    middleCols(Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template middleCols<NV>(start);
+    }
+    
     template<class Mat>
     struct RowsReturn
     {
       typedef typename Mat::template NRowsBlockXpr<NV>::Type Type;
       typedef typename Mat::template ConstNRowsBlockXpr<NV>::Type ConstType;
     };
+    
+    template<typename D>
+    static typename RowsReturn<D>::ConstType
+    middleRows(const Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template middleRows<NV>(start);
+    }
+    
+    template<typename D>
+    static typename RowsReturn<D>::Type
+    middleRows(Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(size);
+      return mat.template middleRows<NV>(start);
+    }
+    
+    template<class Mat>
+    struct BlockReturn
+    {
+      typedef Eigen::Block<Mat, NV, NV> Type;
+      typedef const Eigen::Block<const Mat, NV, NV> ConstType;
+    };
+    
+    template<typename D>
+    static typename BlockReturn<D>::ConstType
+    block(const Eigen::MatrixBase<D> & mat,
+          typename Eigen::DenseBase<D>::Index row_id,
+          typename Eigen::DenseBase<D>::Index col_id,
+          typename Eigen::DenseBase<D>::Index row_size_block = NV,
+          typename Eigen::DenseBase<D>::Index col_size_block = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(row_size_block);
+      PINOCCHIO_UNUSED_VARIABLE(col_size_block);
+      return mat.template block<NV,NV>(row_id,col_id);
+    }
+    
+    template<typename D>
+    static typename BlockReturn<D>::Type
+    block(Eigen::MatrixBase<D> & mat,
+          typename Eigen::DenseBase<D>::Index row_id,
+          typename Eigen::DenseBase<D>::Index col_id,
+          typename Eigen::DenseBase<D>::Index row_size_block = NV,
+          typename Eigen::DenseBase<D>::Index col_size_block = NV)
+    {
+      PINOCCHIO_UNUSED_VARIABLE(row_size_block);
+      PINOCCHIO_UNUSED_VARIABLE(col_size_block);
+      return mat.template block<NV,NV>(row_id,col_id);
+    }
   };
   
   template<>
@@ -177,18 +272,103 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
       typedef typename Mat::SegmentReturnType Type;
       typedef typename Mat::ConstSegmentReturnType ConstType;
     };
+    
+    template<typename D>
+    static typename SegmentReturn<D>::ConstType
+    segment(const Eigen::MatrixBase<D> & mat,
+            typename Eigen::DenseBase<D>::Index start,
+            typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.segment(start,size);
+    }
+    
+    template<typename D>
+    static typename SegmentReturn<D>::Type
+    segment(Eigen::MatrixBase<D> & mat,
+            typename Eigen::DenseBase<D>::Index start,
+            typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.segment(start,size);
+    }
+    
     template<class Mat>
     struct ColsReturn
     {
       typedef typename Mat::ColsBlockXpr Type;
       typedef typename Mat::ConstColsBlockXpr ConstType;
     };
+    
+    template<typename D>
+    static typename ColsReturn<D>::ConstType
+    middleCols(const Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.middleCols(start,size);
+    }
+    
+    template<typename D>
+    static typename ColsReturn<D>::Type
+    middleCols(Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.middleCols(start,size);
+    }
+    
     template<class Mat>
     struct RowsReturn
     {
       typedef typename Mat::RowsBlockXpr Type;
       typedef typename Mat::ConstRowsBlockXpr ConstType;
     };
+    
+    template<typename D>
+    static typename RowsReturn<D>::ConstType
+    middleRows(const Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.middleRows(start,size);
+    }
+    
+    template<typename D>
+    static typename RowsReturn<D>::Type
+    middleRows(Eigen::MatrixBase<D> & mat,
+               typename Eigen::DenseBase<D>::Index start,
+               typename Eigen::DenseBase<D>::Index size)
+    {
+      return mat.middleRows(start,size);
+    }
+    
+    template<class Mat>
+    struct BlockReturn
+    {
+      typedef Eigen::Block<Mat> Type;
+      typedef const Eigen::Block<const Mat> ConstType;
+    };
+    
+    template<typename D>
+    static typename BlockReturn<D>::ConstType
+    block(const Eigen::MatrixBase<D> & mat,
+          typename Eigen::DenseBase<D>::Index row_id,
+          typename Eigen::DenseBase<D>::Index col_id,
+          typename Eigen::DenseBase<D>::Index row_size_block,
+          typename Eigen::DenseBase<D>::Index col_size_block)
+    {
+      return mat.block(row_id,col_id,row_size_block,col_size_block);
+    }
+    
+    template<typename D>
+    static typename BlockReturn<D>::Type
+    block(Eigen::MatrixBase<D> & mat,
+          typename Eigen::DenseBase<D>::Index row_id,
+          typename Eigen::DenseBase<D>::Index col_id,
+          typename Eigen::DenseBase<D>::Index row_size_block,
+          typename Eigen::DenseBase<D>::Index col_size_block)
+    {
+      return mat.block(row_id,col_id,row_size_block,col_size_block);
+    }
   };
 
   template<typename Derived>
@@ -304,46 +484,108 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
     // Const access
     template<typename D>
     typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType 
-    jointConfigSelector(const Eigen::MatrixBase<D>& a) const       { return derived().jointConfigSelector_impl(a); }
+    jointConfigSelector(const Eigen::MatrixBase<D>& a) const
+    { return derived().jointConfigSelector_impl(a); }
+    
     template<typename D>
     typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType 
-    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const   { return a.template segment<NQ>(i_q); }
+    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const
+    { return SizeDepType<NQ>::segment(a,i_q,nq()); }
+    
     // Non-const access
     template<typename D>
     typename SizeDepType<NQ>::template SegmentReturn<D>::Type 
-    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return derived().jointConfigSelector_impl(a); }
+    jointConfigSelector( Eigen::MatrixBase<D>& a) const
+    { return derived().jointConfigSelector_impl(a); }
+    
     template<typename D>
     typename SizeDepType<NQ>::template SegmentReturn<D>::Type 
-    jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NQ>(i_q); }
+    jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const
+    { return SizeDepType<NQ>::segment(a,i_q,nq()); }
 
     /* Acces to dedicated segment in robot config velocity space.  */
     // Const access
     template<typename D>
     typename SizeDepType<NV>::template SegmentReturn<D>::ConstType 
-    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const       { return derived().jointVelocitySelector_impl(a); }
+    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const
+    { return derived().jointVelocitySelector_impl(a); }
+    
     template<typename D>
     typename SizeDepType<NV>::template SegmentReturn<D>::ConstType 
-    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const   { return a.template segment<NV>(i_v); }
+    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const
+    { return SizeDepType<NV>::segment(a,i_v,nv()); }
+    
     // Non-const access
     template<typename D>
     typename SizeDepType<NV>::template SegmentReturn<D>::Type 
-    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return derived().jointVelocitySelector_impl(a); }
+    jointVelocitySelector( Eigen::MatrixBase<D>& a) const
+    { return derived().jointVelocitySelector_impl(a); }
+    
     template<typename D>
     typename SizeDepType<NV>::template SegmentReturn<D>::Type 
-    jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
+    jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const
+    { return SizeDepType<NV>::segment(a,i_v,nv()); }
 
     template<typename D>
     typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
-    jointCols(const Eigen::MatrixBase<D>& A) const       { return derived().jointCols_impl(A); }
+    jointCols(const Eigen::MatrixBase<D>& A) const
+    { return derived().jointCols_impl(A); }
+    
     template<typename D>
     typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
-    jointCols_impl(const Eigen::MatrixBase<D>& A) const       { return A.template middleCols<NV>(i_v); }
+    jointCols_impl(const Eigen::MatrixBase<D>& A) const
+    { return SizeDepType<NV>::middleCols(A,i_v,nv()); }
+    
     template<typename D>
     typename SizeDepType<NV>::template ColsReturn<D>::Type 
-    jointCols(Eigen::MatrixBase<D>& A) const       { return derived().jointCols_impl(A); }
+    jointCols(Eigen::MatrixBase<D>& A) const
+    { return derived().jointCols_impl(A); }
+    
     template<typename D>
     typename SizeDepType<NV>::template ColsReturn<D>::Type 
-    jointCols_impl(Eigen::MatrixBase<D>& A) const       { return A.template middleCols<NV>(i_v); }
+    jointCols_impl(Eigen::MatrixBase<D>& A) const
+    { return SizeDepType<NV>::middleCols(A,i_v,nv()); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
+    jointRows(const Eigen::MatrixBase<D>& A) const
+    { return derived().jointRows_impl(A); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
+    jointRows_impl(const Eigen::MatrixBase<D>& A) const
+    { return SizeDepType<NV>::middleRows(A,i_v,nv()); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template RowsReturn<D>::Type
+    jointRows(Eigen::MatrixBase<D>& A) const
+    { return derived().jointRows_impl(A); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template RowsReturn<D>::Type
+    jointRows_impl(Eigen::MatrixBase<D>& A) const
+    { return SizeDepType<NV>::middleRows(A,i_v,nv()); }
+    
+    /// \brief Returns a block of dimension nv()xnv() located at position i_v,i_v in the matrix Mat
+    template<typename D>
+    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
+    jointBlock(const Eigen::MatrixBase<D> & Mat) const
+    { return derived().jointBlock_impl(Mat); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
+    jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
+    { return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template BlockReturn<D>::Type
+    jointBlock(Eigen::MatrixBase<D> & Mat) const
+    { return derived().jointBlock_impl(Mat); }
+    
+    template<typename D>
+    typename SizeDepType<NV>::template BlockReturn<D>::Type
+    jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
+    { return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
 
   protected:
 
@@ -357,13 +599,14 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
     ///
     /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
     /// objects. Copy is done by calling copy operator.
-    inline JointModelBase( const JointModelBase& clone) { *this = clone; }
+    inline JointModelBase(const JointModelBase & clone)
+    { *this = clone; }
     
     /// Copy operator: protected.
     ///
     /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
     /// objects. 
-    inline JointModelBase& operator=(const JointModelBase& clone)
+    inline JointModelBase & operator=(const JointModelBase & clone)
     {
       i_id = clone.i_id;
       i_q = clone.i_q;

src/multibody/liegroup/liegroup-algo.hxx

@@ -147,6 +147,43 @@ namespace pinocchio
   
   SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStepAlgo);
   
+  template<typename Visitor, typename JointModel> struct dIntegrateStepAlgo;
+  
+  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
+  struct dIntegrateStep
+  : public fusion::JointVisitorBase< dIntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
+  {
+    typedef boost::fusion::vector<const ConfigVectorIn &,
+                                  const TangentVectorIn &,
+                                  JacobianMatrixType &,
+                                  const ArgumentPosition &
+                                  > ArgsType;
+    
+    SE3_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateStepAlgo, dIntegrateStep)
+  };
+  
+  template<typename Visitor, typename JointModel>
+  struct dIntegrateStepAlgo
+  {
+    template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType>
+    static void run(const JointModelBase<JointModel> & jmodel,
+                    const Eigen::MatrixBase<ConfigVectorIn> & q,
+                    const Eigen::MatrixBase<TangentVector> & v,
+                    const Eigen::MatrixBase<JacobianMatrixType> & mat,
+                    const ArgumentPosition & arg)
+    {
+      typedef typename Visitor::LieGroupMap LieGroupMap;
+      
+      typename LieGroupMap::template operation<JointModel>::type lgo;
+      lgo.dIntegrate(jmodel.jointConfigSelector  (q.derived()),
+                     jmodel.jointVelocitySelector(v.derived()),
+                     jmodel.jointBlock(EIGEN_CONST_CAST(JacobianMatrixType,mat)),
+                     arg);
+    }
+  };
+  
+  SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateStepAlgo);
+  
   template<typename Visitor, typename JointModel> struct InterpolateStepAlgo;
   
   template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>

src/multibody/liegroup/liegroup-base.hpp

@@ -94,10 +94,10 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
                                     const Eigen::MatrixBase<Jacobian_t> & J) const;
 
     /**
-     * @brief      Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
+     * @brief      Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
      *
      * @details    This Jacobian corresponds to the Jacobian of \f$ (\bm{q} \oplus \delta \bm{q}) \oplus \bm{v} \f$ with
-     *             \f$ \delta \bm{q} \rightarrow 0 \f$.
+     *             \f$ \delta \bm{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \bm{v} \rightarrow 0 \f$ if arg == ARG1.
      *
      * @param[in]  q    configuration vector.
      * @param[in]  v    tangent vector.
@@ -108,7 +108,29 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
     template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
     void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
                     const Eigen::MatrixBase<Tangent_t>  & v,
-                    const Eigen::MatrixBase<JacobianOut_t>& J) const;
+                    const Eigen::MatrixBase<JacobianOut_t>& J) const
+    {
+      PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
+      return dIntegrate(q,v,J,arg);
+    }
+    
+    /**
+     * @brief      Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
+     *
+     * @details    This Jacobian corresponds to the Jacobian of \f$ (\bm{q} \oplus \delta \bm{q}) \oplus \bm{v} \f$ with
+     *             \f$ \delta \bm{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \bm{v} \rightarrow 0 \f$ if arg == ARG1.
+     *
+     * @param[in]  q    configuration vector.
+     * @param[in]  v    tangent vector.
+     * @param[in] arg  ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
+     *
+     * @param[out] J    the Jacobian of the Integrate operation w.r.t. the argument arg.
+     */
+    template<class Config_t, class Tangent_t, class JacobianOut_t>
+    void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
+                    const Eigen::MatrixBase<Tangent_t>  & v,
+                    const Eigen::MatrixBase<JacobianOut_t> & J,
+                    const ArgumentPosition arg) const;
 
     /**
      * @brief      Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
@@ -213,7 +235,7 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
     PINOCCHIO_DEPRECATED;
 
     /**
-     * @brief      Computes the Jacobian of the difference operation.
+     * @brief      Computes the Jacobian of the difference operation with respect to q0 or q1.
      *
      * @param[in]  q0    the initial configuration vector.
      * @param[in]  q1    the terminal configuration vector.

src/multibody/liegroup/liegroup-base.hxx

@@ -50,12 +50,14 @@ namespace pinocchio {
   }
 
   template <class Derived>
-  template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
+  template<class Config_t, class Tangent_t, class JacobianOut_t>
   void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
-                  const Eigen::MatrixBase<Tangent_t>  & v,
-                  const Eigen::MatrixBase<JacobianOut_t>& J) const
+                                         const Eigen::MatrixBase<Tangent_t>  & v,
+                                         const Eigen::MatrixBase<JacobianOut_t>& J,
+                                         const ArgumentPosition arg) const
   {
-    PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
+    assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
+    
     switch (arg) {
       case ARG0:
         dIntegrate_dq(q,v,J); return;

unittest/joint-configurations.cpp

@@ -86,6 +86,7 @@ BOOST_AUTO_TEST_CASE ( integration_test )
   qs[0] = Eigen::VectorXd::Ones(model.nq);
   qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
   qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
+  qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
  
   qdots[0] = Eigen::VectorXd::Zero(model.nv);
   results[0] = integrate(model,qs[0],qdots[0]);
@@ -93,6 +94,66 @@ BOOST_AUTO_TEST_CASE ( integration_test )
   BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
 }
 
+BOOST_AUTO_TEST_CASE ( diff_integration_test )
+{
+  Model model; buildModel(model);
+  
+  std::vector<Eigen::VectorXd> qs(2);
+  std::vector<Eigen::VectorXd> vs(2);
+  std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
+  std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
+  
+  qs[0] = Eigen::VectorXd::Ones(model.nq);
+  qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
+  qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
+  qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
+  
+  vs[0] = Eigen::VectorXd::Zero(model.nv);
+  vs[1] = Eigen::VectorXd::Ones(model.nv);
+  dIntegrate(model,qs[0],vs[0],results[0],ARG0);
+  
+  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
+  const double eps = 1e-8;
+  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
+  {
+    v_fd[k] = eps;
+    q_fd = integrate(model,qs[0],v_fd);
+    results_fd[0].col(k) = difference(model,qs[0],q_fd)/eps;
+    v_fd[k] = 0.;
+  }
+  BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
+  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
+  
+  dIntegrate(model,qs[0],vs[0],results[1],ARG1);
+  BOOST_CHECK(results[1].isApprox(results[0]));
+  
+  dIntegrate(model,qs[0],vs[1],results[0],ARG0);
+  Eigen::VectorXd q_fd_intermediate(model.nq);
+  Eigen::VectorXd q0_plus_v = integrate(model,qs[0],vs[1]);
+  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
+  {
+    v_fd[k] = eps;
+    q_fd_intermediate = integrate(model,qs[0],v_fd);
+    q_fd = integrate(model,q_fd_intermediate,vs[1]);
+    results_fd[0].col(k) = difference(model,q0_plus_v,q_fd)/eps;
+    v_fd[k] = 0.;
+  }
+  
+  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
+  
+  dIntegrate(model,qs[0],vs[1],results[1],ARG1);
+  v_fd = vs[1];
+  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
+  {
+    v_fd[k] += eps;
+    q_fd = integrate(model,qs[0],v_fd);
+    results_fd[1].col(k) = difference(model,q0_plus_v,q_fd)/eps;
+    v_fd[k] -= eps;
+  }
+  
+  BOOST_CHECK(results[1].isApprox(results_fd[1],sqrt(eps)));
+}
+
 BOOST_AUTO_TEST_CASE ( integrate_difference_test )
 {
  Model model; buildModel(model);