//
// Copyright (c) 2016-2021 CNRS INRIA
//

//
// Copyright (c) INRIA 2026
//
#pragma once

// IWYU pragma: begin_keep
#include <Eigen/Core>

#include <cassert>
#include <vector>

#include "pinocchio/macros.hpp"
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/fwd.hpp"
#include "pinocchio/context.hpp"

#include "pinocchio/utils/check.hpp"

#include "pinocchio/math.hpp"

#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/liegroup.hpp"
// IWYU pragma: end_keep

namespace pinocchio
{

  /// Helpers to use SFINAE for safe candidate selection compare to liegroup-variant-visitors.hxx
  template<typename T, typename = void>
  struct is_lie_group_map : std::false_type
  {
  };

  // Pointer avoids instantiating operation<void>'s body (no complete type needed),
  // while still detecting the nested template via SFINAE
  template<typename T>
  struct is_lie_group_map<T, std::void_t<typename T::template operation<void> *>> : std::true_type
  {
  };

  template<typename T>
  inline constexpr bool is_lie_group_map_v = is_lie_group_map<T>::value;

  /// \name API with return value as argument
  /// \{

  /**
   *
   * @brief      Integrate a configuration vector for the specified model for a tangent vector
   * during one unit time
   *
   * @details This function corresponds to the exponential map of the joint configuration Lie Group.
   *          Its output can be interpreted as the "sum" from the Lie algebra to the joint
   * configuration space \f$ q \oplus v \f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration is performed.
   * @param[in]  q       Initial configuration (size model.nq)
   * @param[in]  v       Joint velocity (size model.nv)
   *
   * @param[out] qout    The integrated configuration (size model.nq)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void integrate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v,
    const Eigen::MatrixBase<ReturnType> & qout);

  /**
   *
   * @brief      Integrate a configuration vector for the specified model for a tangent vector
   * during one unit time
   *
   * @details This function corresponds to the exponential map of the joint configuration Lie Group.
   *          Its output can be interpreted as the "sum" from the Lie algebra to the joint
   * configuration space \f$ q \oplus v \f$.
   *
   * @param[in]  model  Model of the kinematic tree on which the integration is performed.
   * @param[in]  q       Initial configuration (size model.nq)
   * @param[in]  v       Joint velocity (size model.nv)
   *
   * @param[out] qout    The integrated configuration (size model.nq)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename ReturnType>
  void integrate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v,
    const Eigen::MatrixBase<ReturnType> & qout)
  {
    integrate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
      ReturnType>(model, q.derived(), v.derived(), qout.derived());
  }

  /**
   *
   * @brief      Interpolate two configurations for a given model
   *
   * @param[in]  model   Model of the kinematic tree on which the interpolation is performed.
   * @param[in]  q0      Initial configuration vector (size model.nq)
   * @param[in]  q1      Final configuration vector (size model.nq)
   * @param[in]  u       u in [0;1] position along the interpolation.
   *
   * @param[out] qout    The interpolated configuration (q0 if u = 0, q1 if u = 1)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void interpolate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Scalar & u,
    const Eigen::MatrixBase<ReturnType> & qout);

  /**
   *
   * @brief      Interpolate two configurations for a given model
   *
   * @param[in]  model   Model of the kinematic tree on which the interpolation is performed.
   * @param[in]  q0      Initial configuration vector (size model.nq)
   * @param[in]  q1      Final configuration vector (size model.nq)
   * @param[in]  u       u in [0;1] position along the interpolation.
   *
   * @param[out] qout    The interpolated configuration (q0 if u = 0, q1 if u = 1)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType>
  void interpolate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Scalar & u,
    const Eigen::MatrixBase<ReturnType> & qout)
  {
    interpolate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
      ReturnType>(
      model, q0.derived(), q1.derived(), u, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
  }

  /**
   *
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from
   * q0 to q1
   *
   * @details This function corresponds to the log map of the joint configuration Lie Group.
   *          Its output can be interpreted as a difference from the joint configuration space to
   * the Lie algebra \f$ q_1 \ominus q_0 \f$.
   *
   * @param[in]  model   Model of the system on which the difference operation is performed.
   * @param[in]  q0      Initial configuration (size model.nq)
   * @param[in]  q1      Desired configuration (size model.nq)
   *
   * @param[out] dvout   The corresponding velocity (size model.nv)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void difference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Eigen::MatrixBase<ReturnType> & dvout);

  /**
   *
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from
   * q0 to q1
   *
   * @details This function corresponds to the log map of the joint configuration Lie Group.
   *          Its output can be interpreted as a difference from the joint configuration space to
   * the Lie algebra \f$ q_1 \ominus q_0 \f$.
   *
   * @param[in]  model   Model of the system on which the difference operation is performed.
   * @param[in]  q0      Initial configuration (size model.nq)
   * @param[in]  q1      Desired configuration (size model.nq)
   *
   * @param[out] dvout   The corresponding velocity (size model.nv).
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType>
  void difference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Eigen::MatrixBase<ReturnType> & dvout)
  {
    difference<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
      ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, dvout));
  }

  /**
   *
   * @brief      Squared distance between two configuration vectors
   *
   * @param[in]  model      Model of the system on which the squared distance operation is
   * performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   * @param[out] out          The corresponding squared distances for each joint (size
   * model.njoints-1 = number of joints).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void squaredDistance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Eigen::MatrixBase<ReturnType> & out);

  /**
   *
   * @brief      Squared distance between two configuration vectors
   *
   * @param[in]  model      Model of the system on which the squared distance operation is
   * performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   * @param[out] out          The corresponding squared distances for each joint (size
   * model.njoints-1 = number of joints).
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType>
  void squaredDistance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Eigen::MatrixBase<ReturnType> & out)
  {
    squaredDistance<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
      ReturnType>(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(ReturnType, out));
  }

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among provided limits.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   * SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of
   * uniformlySample.
   *
   * @param[in]  model                Model of the system on which the random configuration
   * operation is performed.
   * @param[in]  lowerLimits  Joints lower limits (size model.nq).
   * @param[in]  upperLimits  Joints upper limits (size model.nq).
   * @param[out] qout                  The resulting configuration vector (size model.nq).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void randomConfiguration(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
    const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
    const Eigen::MatrixBase<ReturnType> & qout);

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among provided limits.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   * SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of
   * uniformlySample
   *
   * @param[in]  model               Model of the system on which the random configuration operation
   * is performed.
   * @param[in]  lowerLimits  Joints lower limits (size model.nq).
   * @param[in]  upperLimits  Joints upper limits (size model.nq).
   * @param[out] qout                  The resulting configuration vector (size model.nq).
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    typename ReturnType>
  void randomConfiguration(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
    const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits,
    const Eigen::MatrixBase<ReturnType> & qout)
  {
    randomConfiguration<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2,
      ReturnType>(
      model, lowerLimits.derived(), upperLimits.derived(),
      PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
  }

  /**
   *
   * @brief         Return the neutral configuration element related to the model configuration
   * space.
   *
   * @param[in]     model      Model of the kinematic tree on which the neutral element is computed
   *
   * @param[out]    qout        The neutral configuration element (size model.nq).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ReturnType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void neutral(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ReturnType> & qout);

  /**
   *
   * @brief         Return the neutral configuration element related to the model configuration
   * space.
   *
   * @param[in]     model      Model of the kinematic tree on which the neutral element is computed.
   *
   * @param[out]    qout        The neutral configuration element (size model.nq).
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ReturnType>
  void neutral(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ReturnType> & qout)
  {
    neutral<LieGroupMap, Scalar, Options, JointCollectionTpl, ReturnType>(
      model, PINOCCHIO_EIGEN_CONST_CAST(ReturnType, qout));
  }

  /**
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector or the tangent
   * vector into the tangent space at identity.
   *
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following
   * relationships in the tangent space:
   *           - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q \delta q
   * + o(\delta q)\f$.
   *           - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v \delta v +
   * o(\delta v)\f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q       Initial configuration (size model.nq)
   * @param[in]  v       Joint 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 differentiation is
   * performed.
   * @param[in] op       Operation to apply.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename JacobianMatrixType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  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,
    const AssignmentOperatorType op = SETTO);

  /**
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector or the tangent
   * vector into the tangent space at identity.
   *
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following
   * relationships in the tangent space:
   *           - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v)
   * \delta q + o(\delta q)\f$.
   *           - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v
   * + o(\delta v)\f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint 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 differentiation is
   * performed.
   *
   */
  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)
  {
    dIntegrate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
      JacobianMatrixType>(
      model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg,
      SETTO);
  }

  /**
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector or the tangent
   * vector into the tangent space at identity.
   *
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following
   * relationships in the tangent space:
   *           - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v)
   * \delta q + o(\delta q)\f$.
   *           - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v
   * + o(\delta v)\f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint 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 differentiation is
   * performed.
   * @param[in] op          Operation to apply.
   *
   */
  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,
    const AssignmentOperatorType op)
  {
    dIntegrate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
      JacobianMatrixType>(
      model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg, op);
  }

  /**
   *
   * @brief   Computes the tangentMap that map a small variation of the configuration express in the
   * Lie algebra (a vector of size nv) to a small variation of the configuration in the parametric
   * space (a vector of size nq).
   *
   * @details This map can be interpreted as a vector space Jacobian of the integrate function
   * regarding the variable v in 0. Chained with a Lie group Jacobian in q it allows to recover a
   * vector space Jacobian in the parametric space.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[out] TM           Tangent map in q mapping the Lie algebra with the parametric tangent
   * space.
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentMapMatrixType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void tangentMap(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentMapMatrixType> & TM,
    const AssignmentOperatorType op = SETTO);

  /**
   *
   * @brief   Computes the tangentMap that map a small variation of the configuration express in the
   * Lie algebra (a vector of size nv) to a small variation of the configuration in the parametric
   * space (a vector of size nq).
   *
   * @details This map can be interpreted as a vector space Jacobian of the integrate function
   * regarding the variable v in 0. Chained with a Lie group Jacobian in q it allows to recover a
   * vector space Jacobian in the parametric space.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[out] TM           Tangent map in q mapping the Lie algebra with the parametric tangent
   * space.
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentMapMatrixType>
  void tangentMap(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentMapMatrixType> & TM,
    const AssignmentOperatorType op = SETTO)
  {
    tangentMap<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentMapMatrixType>(
      model, q.derived(), TM.const_cast_derived(), op);
  }

  /**
   *
   * @brief   Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  joint_ids  Joint to condider to compute the tangentMap
   * @param[in]  q                Initial configuration (size model.nq)
   * @param[out] TMc              Compact storage of the tangent map
   * space.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentMapMatrixType>
  void compactTangentMap(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const std::vector<JointIndex> & joint_ids,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentMapMatrixType> & TMc);

  /**
   *
   * @brief   Set the tangentMap in a compact manner in a matrix of size nq x MAX_JOINT_NV.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  joint_ids  Joint to condider to compute the tangentMap
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[out] TMc          Compact storage of the tangent map
   * space.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentMapMatrixType>
  void compactTangentMap(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const std::vector<JointIndex> & joint_ids,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentMapMatrixType> & TMc)
  {
    compactTangentMap<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentMapMatrixType>(
      model, joint_ids, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(TangentMapMatrixType, TMc));
  }

  /**
   *
   * @brief   Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a
   * vector space Jacobian or chain Lie Group derivatives with vector space derivative in a forward
   * manner.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  mat_in       A matrix with range in the Lie algebra (size model.nv, n_cols)
   * @param[out] mat_out      A matrix with range in the parametric space (size model.nq, n_cols)
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename MatrixInType,
    typename MatrixOutType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void tangentMapProduct(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<MatrixInType> & mat_in,
    const Eigen::MatrixBase<MatrixOutType> & mat_out,
    const AssignmentOperatorType op = SETTO);

  /**
   *
   * @brief   Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a
   * vector space Jacobian or chain Lie Group derivatives with vector space derivative in a forward
   * manner.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  mat_in       A matrix with range in the Lie algebra (size model.nv, n_cols)
   * @param[out] mat_out      A matrix with range in the parametric space (size model.nq, n_cols)
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename MatrixInType,
    typename MatrixOutType>
  void tangentMapProduct(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<MatrixInType> & mat_in,
    const Eigen::MatrixBase<MatrixOutType> & mat_out,
    const AssignmentOperatorType op = SETTO)
  {
    tangentMapProduct<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, MatrixInType,
      MatrixOutType>(
      model, q.derived(), mat_in.derived(), PINOCCHIO_EIGEN_CONST_CAST(MatrixOutType, mat_out), op);
  }

  /**
   *
   * @brief   Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a
   * vector space Jacobian or chain Lie Group derivatives with vector space derivative in reverse
   * mode.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  mat_in       A matrix with range in the parametric space (size model.nq, n_cols)
   * @param[out] mat_out      A matrix with range in the Lie algebra (size model.nv, n_cols)
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename MatrixInType,
    typename MatrixOutType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void tangentMapTransposeProduct(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<MatrixInType> & mat_in,
    const Eigen::MatrixBase<MatrixOutType> & mat_out,
    const AssignmentOperatorType op = SETTO);

  /**
   *
   * @brief   Compose the tangentMap with a matrix, e.g., a Lie group Jacobian in order to recover a
   * vector space Jacobian or chain Lie Group derivatives with vector space derivative in reverse
   * mode.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  mat_in       A matrix with range in the parametric space (size model.nq, n_cols)
   * @param[out] mat_out      A matrix with range in the Lie algebra (size model.nv, n_cols)
   * @param[in]  op           Operation to apply.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename MatrixInType,
    typename MatrixOutType>
  void tangentMapTransposeProduct(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<MatrixInType> & mat_in,
    const Eigen::MatrixBase<MatrixOutType> & mat_out,
    const AssignmentOperatorType op = SETTO)
  {
    tangentMapTransposeProduct<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, MatrixInType,
      MatrixOutType>(
      model, q.derived(), mat_in.derived(), PINOCCHIO_EIGEN_CONST_CAST(MatrixOutType, mat_out), op);
  }

  /**
   *
   * @brief   Transport a matrix from the terminal to the initial tangent space of the integrate
   * operation, with respect to the configuration or the velocity arguments.
   *
   * @details This function performs the parallel transportation of an input matrix whose columns
   * are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the tangent
   * space at \f$ q \f$. In other words, this functions transforms a tangent vector expressed at \f$
   * q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of
   * configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent
   * vector. A typical example of parallel transportation is the action operated by a rigid
   * transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. In
   * the context of configuration spaces assimilated to vector spaces, this operation corresponds
   * to Identity. For Lie groups, its corresponds to the canonical vector field transportation.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
   * @param[out] Jin        Input matrix (number of rows = model.nv).
   * @param[out] Jout      Output matrix (same size as Jin).
   * @param[in]  arg        Argument (either ARG0 for q or ARG1 for v) with respect to which the
   * differentation is performed.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename JacobianMatrixType1,
    typename JacobianMatrixType2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void dIntegrateTransport(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v,
    const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
    const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
    const ArgumentPosition arg);

  /**
   *
   * @brief   Transport a matrix from the terminal to the initial tangent space of the integrate
   * operation, with respect to the configuration or the velocity arguments.
   *
   * @details This function performs the parallel transportation of an input matrix whose columns
   * are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the tangent
   * space at \f$ q \f$. In other words, this functions transforms a tangent vector expressed at \f$
   * q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of
   * configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent
   * vector. A typical example of parallel transportation is the action operated by a rigid
   * transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. In
   * the context of configuration spaces assimilated to vector spaces, this operation corresponds
   * to Identity. For Lie groups, its corresponds to the canonical vector field transportation.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
   * @param[out] Jin        Input matrix (number of rows = model.nv).
   * @param[out] Jout      Output matrix (same size as Jin).
   * @param[in]  arg        Argument (either ARG0 for q or ARG1 for v) with respect to which the
   * differentation is performed.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename JacobianMatrixType1,
    typename JacobianMatrixType2>
  void dIntegrateTransport(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v,
    const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
    const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
    const ArgumentPosition arg)
  {
    dIntegrateTransport<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
      JacobianMatrixType1, JacobianMatrixType2>(
      model, q.derived(), v.derived(), Jin.derived(),
      PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2, Jout), arg);
  }

  /**
   *
   * @brief   Transport in place a matrix from the terminal to the initial tangent space of the
   * integrate operation, with respect to the configuration or the velocity arguments.
   *
   * @details This function performs the parallel transportation of an input matrix whose columns
   * are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the tangent
   * space at \f$ q \f$. In other words, this functions transforms a tangent vector expressed at \f$
   * q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of
   * configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent
   * vector. A typical example of parallel transportation is the action operated by a rigid
   * transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. In
   * the context of configuration spaces assimilated to vector spaces, this operation corresponds
   * to Identity. For Lie groups, its corresponds to the canonical vector field transportation.
   *
   * @param[in]     model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]     q            Initial configuration (size model.nq)
   * @param[in]     v            Joint velocity (size model.nv)
   * @param[in,out] J            Input/output matrix (number of rows = model.nv).
   * @param[in]     arg        Argument (either ARG0 for q or ARG1 for v) with respect to which the
   * differentation is performed.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename JacobianMatrixType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void dIntegrateTransport(
    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   Transport in place a matrix from the terminal to the initial tangent space of the
   * integrate operation, with respect to the configuration or the velocity arguments.
   *
   * @details This function performs the parallel transportation of an input matrix whose columns
   * are expressed in the tangent space of the integrated element \f$ q \oplus v \f$, to the tangent
   * space at \f$ q \f$. In other words, this functions transforms a tangent vector expressed at \f$
   * q \oplus v \f$ to a tangent vector expressed at \f$ q \f$, considering that the change of
   * configuration between \f$ q \oplus v \f$ and \f$ q \f$ may alter the value of this tangent
   * vector. A typical example of parallel transportation is the action operated by a rigid
   * transformation \f$ M \in \text{SE}(3)\f$ on a spatial velocity \f$ v \in \text{se}(3)\f$. In
   * the context of configuration spaces assimilated to vector spaces, this operation corresponds
   * to Identity. For Lie groups, its corresponds to the canonical vector field transportation.
   *
   * @param[in]     model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]     q            Initial configuration (size model.nq)
   * @param[in]     v            Joint velocity (size model.nv)
   * @param[in,out] J            Input/output matrix (number of rows = model.nv).
   * @param[in]     arg        Argument (either ARG0 for q or ARG1 for v) with respect to which the
   * differentation is performed.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    typename JacobianMatrixType>
  void dIntegrateTransport(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v,
    const Eigen::MatrixBase<JacobianMatrixType> & J,
    const ArgumentPosition arg)
  {
    dIntegrateTransport<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType,
      JacobianMatrixType>(
      model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, J), arg);
  }

  /**
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector into the
   * tangent space at identity.
   *
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the
   * following relationships in the tangent space:
   *           - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) =
   * J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$.
   *           - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) =
   * J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Joint velocity (size model.nv)
   * @param[out] J            Jacobian of the Difference operation, either with respect to q0 or q1
   * (size model.nv x model.nv).
   * @param[in]  arg        Argument (either q0 or q1) with respect to which the differentiation is
   * performed.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVector1,
    typename ConfigVector2,
    typename JacobianMatrix,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void dDifference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVector1> & q0,
    const Eigen::MatrixBase<ConfigVector2> & q1,
    const Eigen::MatrixBase<JacobianMatrix> & J,
    const ArgumentPosition arg);

  /**
   *
   * @brief   Computes the Jacobian of a small variation of the configuration vector into the
   * tangent space at identity.
   *
   * @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
   *          it is expressed in the tangent space only, not the configuration space.
   *          Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the
   * following relationships in the tangent space:
   *           - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) =
   * J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$.
   *           - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) =
   * J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$.
   *
   * @param[in]  model   Model of the kinematic tree on which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Joint velocity (size model.nv)
   * @param[out] J            Jacobian of the Difference operation, either with respect to q0 or q1
   * (size model.nv x model.nv).
   * @param[in]  arg        Argument (either q0 or q1) with respect to which the differentiation is
   * performed.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVector1,
    typename ConfigVector2,
    typename JacobianMatrix>
  void dDifference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVector1> & q0,
    const Eigen::MatrixBase<ConfigVector2> & q1,
    const Eigen::MatrixBase<JacobianMatrix> & J,
    const ArgumentPosition arg)
  {
    dDifference<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector1, ConfigVector2,
      JacobianMatrix>(
      model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J), arg);
  }
  /**
   *
   * @brief      Overall squared distance between two configuration vectors
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
   * @return     The squared distance between the two configurations
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  Scalar squaredDistanceSum(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1);

  /**
   *
   * @brief      Overall squared distance between two configuration vectors, namely \f$ || q_{1}
   * \ominus q_{0} ||_2^{2} \f$.
   *
   * @param[in]  model  Model of the kinematic tree
   * @param[in]  q0         Configuration from (size model.nq)
   * @param[in]  q1         Configuration to (size model.nq)
   *
   * @return     The squared distance between the two configurations q0 and q1.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  Scalar squaredDistanceSum(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return squaredDistanceSum<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q0.derived(), q1.derived());
  }

  /**
   *
   * @brief      Distance between two configuration vectors, namely \f$ || q_{1} \ominus q_{0} ||_2
   * \f$.
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
   * @return     The distance between the two configurations q0 and q1.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  Scalar distance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1);

  /**
   *
   * @brief      Distance between two configuration vectors
   *
   * @param[in]  model      Model we want to compute the distance
   * @param[in]  q0         Configuration 0 (size model.nq)
   * @param[in]  q1         Configuration 1 (size model.nq)
   *
   * @return     The distance between the two configurations q0 and q1.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  Scalar distance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return distance<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q0.derived(), q1.derived());
  }

  /**
   *
   * @brief         Normalize a configuration vector.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in,out] qout               Configuration to normalize (size model.nq).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  void normalize(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & qout);

  /**
   *
   * @brief         Normalize a configuration vector.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in,out] qout               Configuration to normalize (size model.nq).
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void normalize(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & qout)
  {
    normalize<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
      model, PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout));
  }

  /**
   *
   * @brief         Check whether a configuration vector is normalized within the given precision
   * provided by prec.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in]     q          Configuration to check (size model.nq).
   * @param[in]     prec       Precision.
   *
   * @return     Whether the configuration is normalized or not, within the given precision.
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  bool isNormalized(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());

  /**
   *
   * @brief         Check whether a configuration vector is normalized within the given precision
   * provided by prec.
   *
   * @param[in]     model      Model of the kinematic tree.
   * @param[in]     q          Configuration to check (size model.nq).
   * @param[in]     prec       Precision.
   *
   * @return     Whether the configuration is normalized or not, within the given precision.
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  bool isNormalized(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
  {
    return isNormalized<LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType>(
      model, q, prec);
  }

  /**
   *
   * @brief         Return true if the given configurations are equivalents, within the given
   * precision.
   * @remarks       Two configurations can be equivalent but not equally coefficient wise (e.g two
   * quaternions with opposite coefficients give rise to the same orientation, i.e. they are
   * equivalent.).
   *
   * @param[in]     model     Model of the kinematic tree.
   * @param[in]     q1        The first configuraiton to compare.
   * @param[in]     q2        The second configuration to compare.
   * @param[in]     prec      precision of the comparison.
   *
   * @return     Whether the configurations are equivalent or not, within the given precision.
   *
   */

  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  bool isSameConfiguration(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q1,
    const Eigen::MatrixBase<ConfigVectorIn2> & q2,
    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());

  /**
   *
   * @brief         Return true if the given configurations are equivalents, within the given
   * precision.
   * @remarks       Two configurations can be equivalent but not equally coefficient wise (e.g two
   * quaternions with opposite coefficients give rise to the same orientation, i.e. they are
   * equivalent.).
   *
   * @param[in]     model     Model of the kinematic tree.
   * @param[in]     q1           The first configuraiton to compare
   * @param[in]     q2           The second configuration to compare
   * @param[in]     prec      precision of the comparison.
   *
   * @return     Whether the configurations are equivalent or not
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  bool isSameConfiguration(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q1,
    const Eigen::MatrixBase<ConfigVectorIn2> & q2,
    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
  {
    return isSameConfiguration<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q1.derived(), q2.derived(), prec);
  }

  /**
   *
   * @brief         Return the Jacobian of the integrate function for the components of the config
   * vector.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @param[in]     q          The joint configuration (vector dim model.nq)
   * @param[out]    jacobian   The Jacobian of the integrate operation.
   *
   * @details       This function is often required for the numerical solvers that are working on
   * the tangent of the configuration space, instead of the configuration space itself.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVector,
    typename JacobianMatrix>
  void integrateCoeffWiseJacobian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVector> & q,
    const Eigen::MatrixBase<JacobianMatrix> & jacobian);

  /**
   *
   * @brief         Return the Jacobian of the integrate function for the components of the config
   * vector.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @param[in]     q          The joint configuration (vector dim model.nq)
   * @param[out]    jacobian   The Jacobian of the integrate operation.
   *
   * @details       This function is often required for the numerical solvers that are working on
   * the tangent of the configuration space, instead of the configuration space itself.
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVector,
    typename JacobianMatrix>
  void integrateCoeffWiseJacobian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVector> & q,
    const Eigen::MatrixBase<JacobianMatrix> & jacobian)
  {
    integrateCoeffWiseJacobian<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVector, JacobianMatrix>(
      model, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian));
  }

  /**
   *
   * @brief         Returns the Lie group associated to the model. It is the cartesian product of
   * the lie groups of all its joints.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @param[out]    lgo            The cartesian Lie group object.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl>
  void lieGroup(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    typename LieGroup_t::template operationProduct<Scalar, Options>::type & lgo);

  /**
   *
   * @brief         Returns the Lie group associated to the model. It is the cartesian product of
   * the lie groups of all its joints.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @param[out]    lgo            The cartesian Lie group object.
   *
   */
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void lieGroup(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    typename LieGroupMap::template operationProduct<Scalar, Options>::type & lgo)
  {
    lieGroup<LieGroupMap, Scalar, Options, JointCollectionTpl>(model, lgo);
  }

  /**
   *
   * @brief         Return two vector where for each, the idx_v and v associated to the
   * same atomic joint is given.
   *
   * @param[in]     model            Model of the kinematic tree.
   * @param[in]     joint_ids        Joint to condider to compute the tangentMap
   * @param[out]    nvs              For each id give the nv of the associated joint
   * @param[out]    idx_vs           For each id give the idx_v of the associated joint
   *
   * @details       This function is often required for the numerical solvers that are working on
   * the tangent of the configuration space, instead of the configuration space itself.
   *
   */
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void getTangentToConfigurationSparsitySegment(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const std::vector<JointIndex> & joint_ids,
    std::vector<int> & nvs,
    std::vector<int> & idx_vs);

  /// \}

  /// \name API that allocates memory
  /// \{

  /**
   *
   * @brief      Integrate a configuration vector for the specified model for a tangent vector
   * during one unit time
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
   *
   * @return     The integrated configuration (size model.nq)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v);

  /**
   *
   * @brief      Integrate a configuration vector for the specified model for a tangent vector
   * during one unit time.
   *
   * @param[in]  model   Model of the kinematic tree on which the integration operation is
   * performed.
   * @param[in]  q            Initial configuration (size model.nq)
   * @param[in]  v            Joint velocity (size model.nv)
   *
   * @return     The integrated configuration (size model.nq)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v)
  {
    return integrate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>(
      model, q.derived(), v.derived());
  }

  /**
   *
   * @brief      Interpolate two configurations for a given model.
   *
   * @param[in]  model   Model of the kinematic tree on which the interpolation operation is
   * performed.
   * @param[in]  q0          Initial configuration vector (size model.nq)
   * @param[in]  q1          Final configuration vector (size model.nq)
   * @param[in]  u            u in [0;1] position along the interpolation.
   *
   * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Scalar & u);

  /**
   *
   * @brief      Interpolate two configurations for a given model.
   *
   * @param[in]  model   Model of the kinematic tree on which the interpolation operation is
   * performed.
   * @param[in]  q0          Initial configuration vector (size model.nq)
   * @param[in]  q1          Final configuration vector (size model.nq)
   * @param[in]  u            u in [0;1] position along the interpolation.
   *
   * @return     The interpolated configuration (q0 if u = 0, q1 if u = 1)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) interpolate(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1,
    const Scalar & u)
  {
    return interpolate<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q0.derived(), q1.derived(), u);
  }

  /**
   *
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from
   * q0 to q1
   *
   * @param[in]  model   Model of the kinematic tree on which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Finial configuration (size model.nq)
   *
   * @return     The corresponding velocity (size model.nv)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1);

  /**
   *
   * @brief      Compute the tangent vector that must be integrated during one unit time to go from
   * q0 to q1.
   *
   * @param[in]  model   Model of the kinematic tree on which the difference operation is performed.
   * @param[in]  q0          Initial configuration (size model.nq)
   * @param[in]  q1          Final configuration (size model.nq)
   *
   * @return     The corresponding velocity (size model.nv)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) difference(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return difference<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q0.derived(), q1.derived());
  }

  /**
   *
   * @brief      Squared distance between two configurations.
   *
   * @param[in]  model      Model of the kinematic tree on which the squared distance operation is
   * performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   *
   * @return     The corresponding squared distances for each joint (size model.njoints-1,
   * corresponding to the number of joints)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1);

  /**
   *
   * @brief      Squared distance between two configuration vectors
   *
   * @param[in]  model      Model of the kinematic tree on which the squared distance operation is
   * performed.
   * @param[in]  q0             Configuration 0 (size model.nq)
   * @param[in]  q1             Configuration 1 (size model.nq)
   *
   * @return     The corresponding squared distances for each joint (size model.njoints-1,
   * corresponding to the number of joints)
   *
   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorIn1) squaredDistance(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const Eigen::MatrixBase<ConfigVectorIn1> & q0,
    const Eigen::MatrixBase<ConfigVectorIn2> & q1)
  {
    return squaredDistance<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, q0.derived(), q1.derived());
  }

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among given limits.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   * SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of
   * uniformlySample
   *
   * @param[in]  model                Model of the kinematic tree on which the uniform sampling
   * operation is performed.
   * @param[in]  lowerLimits   Joints lower limits (size model.nq).
   * @param[in]  upperLimits   Joints upper limits (size model.nq).
   *
   * @return     The resulting configuration vector (size model.nq).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
    (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
    randomConfiguration(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits);

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among provided limits.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning     If limits are infinite, exceptions may be thrown in the joint implementation of
   uniformlySample
   *
   * @param[in]  model               Model of the kinematic tree on which the uniform sampling
   operation is performed.
   * @param[in]  lowerLimits  Joints lower limits (size model.nq).
   * @param[in]  upperLimits  Joints upper limits (size model.nq).
   *
   * @return     The resulting configuration vector (size model.nq)

   */
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorIn1,
    typename ConfigVectorIn2>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
    (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
    randomConfiguration(
      const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
      const Eigen::MatrixBase<ConfigVectorIn1> & lowerLimits,
      const Eigen::MatrixBase<ConfigVectorIn2> & upperLimits)
  {
    return randomConfiguration<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, lowerLimits.derived(), upperLimits.derived());
  }

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among the joint limits of the
   * specified Model.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   * SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning    If limits are infinite (no one specified when adding a body or no modification
   * directly in my_model.{lowerPositionLimit,upperPositionLimit}, exceptions may be thrown in the
   * joint implementation of uniformlySample
   *
   * @param[in]  model   Model of the kinematic tree on which the uniform sampling operation is
   * performed.
   *
   * @return     The resulting configuration vector (size model.nq)
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
    (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
    randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);

  /**
   *
   * @brief      Generate a configuration vector uniformly sampled among the joint limits of the
   * specified Model.
   *
   * @remarks    Limits are not taken into account for rotational transformations (typically
   * SO(2),SO(3)), because they are by definition unbounded.
   *
   * @warning    If limits are infinite (no one specified when adding a body or no modification
   * directly in my_model.{lowerPositionLimit,upperPositionLimit}, exceptions may be thrown in the
   * joint implementation of uniformlySample
   *
   * @param[in]  model   Model of the kinematic tree on which the uniform sampling operation is
   * performed.
   *
   * @return     The resulting configuration vector (size model.nq).
   *
   */
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS(
    (typename ModelTpl<Scalar, Options, JointCollectionTpl>::ConfigVectorType))
    randomConfiguration(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
  {
    return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
  }

  /**
   *
   * @brief         Return the neutral configuration element related to the model configuration
   * space.
   *
   * @param[in]     model      Model of the kinematic tree on which the neutral element is computed.
   *
   * @return        The neutral configuration element (size model.nq).
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    std::enable_if_t<is_lie_group_map_v<LieGroup_t>, int> = 0>
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);

  /**
   * @brief         Return the neutral configuration element related to the model configuration
   * space.
   *
   * @param[in]     model      Model of the kinematic tree on which the neutral element is computed.
   *
   * @return        The neutral configuration element (size model.nq).
   */
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options>
  neutral(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
  {
    return neutral<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
  }

  /**
   *
   * @brief         Returns the Lie group associated to the model. It is the cartesian product of
   * the lie groups of all its joints.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @returns The cartesian Lie group object.
   *
   */
  template<
    typename LieGroup_t,
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl>
  typename LieGroup_t::template operationProduct<Scalar, Options>::type
  lieGroup(const ModelTpl<Scalar, Options, JointCollectionTpl> & model);

  /**
   *
   * @brief         Returns the Lie group associated to the model. It is the cartesian product of
   * the lie groups of all its joints.
   *
   * @param[in]     model          Model of the kinematic tree.
   * @returns The cartesian Lie group object.
   *
   */
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  typename LieGroupMap::template operationProduct<Scalar, Options>::type
  lieGroup(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
  {
    return lieGroup<LieGroupMap, Scalar, Options, JointCollectionTpl>(model);
  }

  /// \}

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/joint-configuration.hxx"
// IWYU pragma: end_exports
