Program Listing for File liegroup-algo.hxx

↰ Return to documentation for file (include/pinocchio/multibody/liegroup/liegroup-algo.hxx)

//
// Copyright (c) 2018-2020 CNRS INRIA
//

#ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__
#define __pinocchio_multibody_liegroup_liegroup_algo_hxx__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"

namespace pinocchio
{

  namespace details
  {
    template<typename Algo>
    struct Dispatch
    {
      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsType>
      static void run(const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel,
                      ArgsType args)
      {
        for (size_t i = 0; i < jmodel.joints.size(); ++i)
          Algo::run(jmodel.joints[i], args);
      }
    };

#define PINOCCHIO_DETAILS_WRITE_ARGS_0(JM) const JointModelBase<JM> & jmodel
#define PINOCCHIO_DETAILS_WRITE_ARGS_1(JM) PINOCCHIO_DETAILS_WRITE_ARGS_0(JM), typename boost::fusion::result_of::at_c<ArgsType, 0>::type a0
#define PINOCCHIO_DETAILS_WRITE_ARGS_2(JM) PINOCCHIO_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1
#define PINOCCHIO_DETAILS_WRITE_ARGS_3(JM) PINOCCHIO_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2
#define PINOCCHIO_DETAILS_WRITE_ARGS_4(JM) PINOCCHIO_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3
#define PINOCCHIO_DETAILS_WRITE_ARGS_5(JM) PINOCCHIO_DETAILS_WRITE_ARGS_4(JM), typename boost::fusion::result_of::at_c<ArgsType, 4>::type a4

#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Algo)                          \
  template <typename Visitor, typename JointCollection>                       \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {           \
    typedef typename Visitor::ArgsType ArgsType;                              \
    static void run (PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelCompositeTpl<JointCollection>))         \
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0)); } \
  }

#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_2(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
    static void run (PINOCCHIO_DETAILS_WRITE_ARGS_2(JointModelCompositeTpl<JointCollection>))         \
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1)); } \
  }

#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
    static void run (PINOCCHIO_DETAILS_WRITE_ARGS_3(JointModelCompositeTpl<JointCollection>))         \
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2)); } \
  }

#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
    static void run (PINOCCHIO_DETAILS_WRITE_ARGS_4(JointModelCompositeTpl<JointCollection>))         \
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3)); } \
  }

#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(Algo)                 \
  template <typename Visitor, typename JointCollection>                    \
  struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > {        \
    typedef typename Visitor::ArgsType ArgsType;                  \
    static void run (PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModelCompositeTpl<JointCollection>))         \
    { ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3,a4)); } \
  }

#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
    template<typename JointModel>                                              \
    static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModel))                     \
    { AlgoDispatch<JointModel>::run(jmodel, a0); }                   \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };

#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_2(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
    template<typename JointModel>                                              \
    static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_2(JointModel))                     \
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1); }               \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };

#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
    template<typename JointModel>                                              \
    static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_3(JointModel))                     \
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2); }           \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };

#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
    template<typename JointModel>                                              \
    static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_4(JointModel))                     \
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3); }       \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };

#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(Algo, Visitor)                      \
    typedef LieGroup_t LieGroupMap;                                              \
    template<typename JointModel>                                              \
    static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModel))                     \
    { AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3, a4); }         \
    template<typename JointModel>                            \
    struct AlgoDispatch : Algo<Visitor, JointModel>                            \
    { using Algo<Visitor, JointModel>::run; };

  } // namespace details

  template<typename Visitor, typename JointModel> struct IntegrateStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename ConfigVectorOut>
  struct IntegrateStep
  : public fusion::JointUnaryVisitorBase< IntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,ConfigVectorOut> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  ConfigVectorOut &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(IntegrateStepAlgo, IntegrateStep)
  };

  template<typename Visitor, typename JointModel>
  struct IntegrateStepAlgo
  {
    template<typename ConfigVectorIn, typename TangentVector, typename ConfigVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Eigen::MatrixBase<TangentVector> & v,
                    const Eigen::MatrixBase<ConfigVectorOut> & result)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.integrate(jmodel.jointConfigSelector  (q.derived()),
                    jmodel.jointVelocitySelector(v.derived()),
                    jmodel.jointConfigSelector  (PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,result)));
    }
  };

  PINOCCHIO_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::JointUnaryVisitorBase< dIntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  JacobianMatrixType &,
                                  const ArgumentPosition &,
                                  const AssignmentOperatorType&
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(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,
                    const AssignmentOperatorType& op)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.dIntegrate(jmodel.jointConfigSelector  (q.derived()),
                     jmodel.jointVelocitySelector(v.derived()),
                     jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
                     arg, op);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateStepAlgo);

  template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixInType, typename JacobianMatrixOutType>
  struct dIntegrateTransportStep
  : public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixInType,JacobianMatrixOutType> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  const JacobianMatrixInType &,
                                  JacobianMatrixOutType &,
                                  const ArgumentPosition &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateTransportStepAlgo, dIntegrateTransportStep)
  };

  template<typename Visitor, typename JointModel>
  struct dIntegrateTransportStepAlgo
  {
    template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixInType, typename JacobianMatrixOutType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const Eigen::MatrixBase<TangentVector> & v,
                    const Eigen::MatrixBase<JacobianMatrixInType> & mat_in,
                    const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out,
                    const ArgumentPosition & arg)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.dIntegrateTransport(jmodel.jointConfigSelector  (q.derived()),
                              jmodel.jointVelocitySelector(v.derived()),
                              jmodel.jointRows(mat_in.derived()),
                              jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)),
                              arg);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo);


  template<typename Visitor, typename JointModel> struct dIntegrateTransportInPlaceStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
  struct dIntegrateTransportInPlaceStep
  : public fusion::JointUnaryVisitorBase< dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const TangentVectorIn &,
                                  JacobianMatrixType &,
                                  const ArgumentPosition &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportInPlaceStepAlgo,
                                            dIntegrateTransportInPlaceStep)
  };

  template<typename Visitor, typename JointModel>
  struct dIntegrateTransportInPlaceStepAlgo
  {
    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.dIntegrateTransport(jmodel.jointConfigSelector  (q.derived()),
                              jmodel.jointVelocitySelector(v.derived()),
                              jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
                              arg);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportInPlaceStepAlgo);

  template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;

  template<typename LieGroup_t, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
  struct dDifferenceStep
  : public fusion::JointUnaryVisitorBase< dDifferenceStep<LieGroup_t,ConfigVector1,ConfigVector2,JacobianMatrix> >
  {
    typedef boost::fusion::vector<const ConfigVector1 &,
                                  const ConfigVector2 &,
                                  JacobianMatrix &,
                                  const ArgumentPosition &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dDifferenceStepAlgo, dDifferenceStep)
  };

  template<typename Visitor, typename JointModel>
  struct dDifferenceStepAlgo
  {
    template<typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVector1> & q0,
                    const Eigen::MatrixBase<ConfigVector2> & q1,
                    const Eigen::MatrixBase<JacobianMatrix> & mat,
                    const ArgumentPosition & arg)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.dDifference(jmodel.jointConfigSelector(q0.derived()),
                      jmodel.jointConfigSelector(q1.derived()),
                      jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,mat)),
                      arg);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dDifferenceStepAlgo);

  template<typename Visitor, typename JointModel> struct InterpolateStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
  struct InterpolateStep
  : public fusion::JointUnaryVisitorBase< InterpolateStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar,ConfigVectorOut  > >
  {
    typedef boost::fusion::vector<const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  const Scalar &,
                                  ConfigVectorOut &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(InterpolateStepAlgo, InterpolateStep)
  };

  template<typename Visitor, typename JointModel>
  struct InterpolateStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Scalar & u,
                    const Eigen::MatrixBase<ConfigVectorOut> & result)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.interpolate(jmodel.jointConfigSelector(q0.derived()),
                      jmodel.jointConfigSelector(q1.derived()),
                      u,
                      jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,result)));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(InterpolateStepAlgo);

  template<typename Visitor, typename JointModel> struct DifferenceStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut>
  struct DifferenceStep
  : public fusion::JointUnaryVisitorBase< DifferenceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,TangentVectorOut> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  TangentVectorOut &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(DifferenceStepAlgo, DifferenceStep)
  };

  template<typename Visitor, typename JointModel>
  struct DifferenceStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename TangentVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Eigen::MatrixBase<TangentVectorOut> & result)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.difference(jmodel.jointConfigSelector(q0.derived()),
                     jmodel.jointConfigSelector(q1.derived()),
                     jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut,result)));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(DifferenceStepAlgo);

  template<typename Visitor, typename JointModel> struct SquaredDistanceStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut>
  struct SquaredDistanceStep
  : public fusion::JointUnaryVisitorBase<SquaredDistanceStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,DistanceVectorOut> >
  {
    typedef boost::fusion::vector<const JointIndex &,
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  DistanceVectorOut &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(SquaredDistanceStepAlgo, SquaredDistanceStep)
  };

  template<typename Visitor, typename JointModel>
  struct SquaredDistanceStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2, typename DistanceVectorOut>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const JointIndex & i,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    const Eigen::MatrixBase<DistanceVectorOut> & distances)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;
      typename LieGroupMap::template operation<JointModel>::type lgo;
      DistanceVectorOut & distances_ = PINOCCHIO_EIGEN_CONST_CAST(DistanceVectorOut,distances);
      distances_[(Eigen::DenseIndex)i] += lgo.squaredDistance(jmodel.jointConfigSelector(q0.derived()),
                                                              jmodel.jointConfigSelector(q1.derived()));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(SquaredDistanceStepAlgo);

  template<typename Visitor, typename JointModel> struct SquaredDistanceSumStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
  struct SquaredDistanceSumStep
  : public fusion::JointUnaryVisitorBase<SquaredDistanceSumStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  Scalar &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(SquaredDistanceSumStepAlgo, SquaredDistanceSumStep)
  };

  template<typename Visitor, typename JointModel>
  struct SquaredDistanceSumStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
                    typename ConfigVectorIn1::Scalar & squaredDistance)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;
      typename LieGroupMap::template operation<JointModel>::type lgo;
      squaredDistance += lgo.squaredDistance(jmodel.jointConfigSelector(q0.derived()),
                                             jmodel.jointConfigSelector(q1.derived()));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(SquaredDistanceSumStepAlgo);

  template<typename Visitor, typename JointModel> struct RandomConfigurationStepAlgo;

  template<typename LieGroup_t,typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2>
  struct RandomConfigurationStep
  : public fusion::JointUnaryVisitorBase< RandomConfigurationStep<LieGroup_t,ConfigVectorOut,ConfigVectorIn1,ConfigVectorIn2> >
  {
    typedef boost::fusion::vector<ConfigVectorOut &,
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &
                                  > ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(RandomConfigurationStepAlgo, RandomConfigurationStep)
  };

  template<typename Visitor, typename JointModel>
  struct RandomConfigurationStepAlgo
  {
    template<typename ConfigVectorOut, typename ConfigVectorIn1, typename ConfigVectorIn2>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorOut> & q,
                    const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
                    const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.randomConfiguration(jmodel.jointConfigSelector(lowerLimits.derived()),
                              jmodel.jointConfigSelector(upperLimits.derived()),
                              jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,q)));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(RandomConfigurationStepAlgo);

  template<typename Visitor, typename JointModel> struct NormalizeStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorType>
  struct NormalizeStep
  : public fusion::JointUnaryVisitorBase< NormalizeStep<LieGroup_t,ConfigVectorType> >
  {
    typedef boost::fusion::vector<ConfigVectorType &> ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(NormalizeStepAlgo, NormalizeStep)
  };

  template<typename Visitor, typename JointModel>
  struct NormalizeStepAlgo
  {
    template<typename ConfigVectorType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & qout)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      lgo.normalize(jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,qout)));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NormalizeStepAlgo);

  template<typename Visitor, typename JointModel> struct IsNormalizedStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn, typename Scalar>
  struct IsNormalizedStep
  : public fusion::JointUnaryVisitorBase< IsNormalizedStep<LieGroup_t,ConfigVectorIn,Scalar> >
  {
    typedef boost::fusion::vector<const ConfigVectorIn &,
                                  const Scalar&,
                                  bool &> ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_3(IsNormalizedStepAlgo, IsNormalizedStep)
  };

  template<typename Visitor, typename JointModel>
  struct IsNormalizedStepAlgo
  {
    template<typename ConfigVectorIn>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorIn> & q,
                    const typename ConfigVectorIn::Scalar& prec,
                    bool& res)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IsNormalizedStepAlgo);

  template<typename Visitor, typename JointModel> struct IsSameConfigurationStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar>
  struct IsSameConfigurationStep
  : public fusion::JointUnaryVisitorBase< IsSameConfigurationStep<LieGroup_t,ConfigVectorIn1,ConfigVectorIn2,Scalar> >
  {
    typedef boost::fusion::vector<bool &,
                                  const ConfigVectorIn1 &,
                                  const ConfigVectorIn2 &,
                                  const Scalar &> ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(IsSameConfigurationStepAlgo, IsSameConfigurationStep)
  };

  template<typename Visitor, typename JointModel>
  struct IsSameConfigurationStepAlgo
  {
    template<typename ConfigVectorIn1, typename ConfigVectorIn2>
    static void run(const JointModelBase<JointModel> & jmodel,
                    bool & isSame,
                    const Eigen::MatrixBase<ConfigVectorIn1> & q1,
                    const Eigen::MatrixBase<ConfigVectorIn2> & q2,
                    const typename ConfigVectorIn1::Scalar & prec)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      isSame &= lgo.isSameConfiguration(jmodel.jointConfigSelector(q1.derived()),
                                        jmodel.jointConfigSelector(q2.derived()),
                                        prec);
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(IsSameConfigurationStepAlgo);

  template<typename Visitor, typename JointModel> struct NeutralStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorType>
  struct NeutralStep
  : public fusion::JointUnaryVisitorBase< NeutralStep<LieGroup_t,ConfigVectorType> >
  {
    typedef boost::fusion::vector<ConfigVectorType &> ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(NeutralStepAlgo, NeutralStep)
  };

  template<typename Visitor, typename JointModel>
  struct NeutralStepAlgo
  {
    template<typename ConfigVectorType>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & neutral_elt)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typename LieGroupMap::template operation<JointModel>::type lgo;
      jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType,neutral_elt)) = lgo.neutral();
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo);

  template<typename Visitor, typename JointModel> struct IntegrateCoeffWiseJacobianStepAlgo;

  template<typename LieGroup_t, typename ConfigVectorType, typename JacobianMatrix>
  struct IntegrateCoeffWiseJacobianStep
  : public fusion::JointUnaryVisitorBase< IntegrateCoeffWiseJacobianStep<LieGroup_t,ConfigVectorType,JacobianMatrix> >
  {
    typedef boost::fusion::vector<const ConfigVectorType &, JacobianMatrix &> ArgsType;

    PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_2(IntegrateCoeffWiseJacobianStepAlgo,
                                      IntegrateCoeffWiseJacobianStep)
  };

  template<typename Visitor, typename JointModel>
  struct IntegrateCoeffWiseJacobianStepAlgo
  {
    template<typename ConfigVectorType, typename JacobianMatrix>
    static void run(const JointModelBase<JointModel> & jmodel,
                    const Eigen::MatrixBase<ConfigVectorType> & q,
                    const Eigen::MatrixBase<JacobianMatrix> & jacobian)
    {
      typedef typename Visitor::LieGroupMap LieGroupMap;

      typedef typename LieGroupMap::template operation<JointModel>::type LieGroup;
      LieGroup lgo;
      lgo.integrateCoeffWiseJacobian(jmodel.jointConfigSelector(q.derived()),
                                     PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,jacobian).template block<LieGroup::NQ,LieGroup::NV>(jmodel.idx_q(),jmodel.idx_v(),jmodel.nq(),jmodel.nv()));
    }
  };

  PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_2(IntegrateCoeffWiseJacobianStepAlgo);

}

#endif // ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__