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

#pragma once

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

#include <cassert>
#include <cstddef>
#include <type_traits>
#include <vector>

#include <boost/fusion/container/vector.hpp>

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

#include "pinocchio/math.hpp"

#include "pinocchio/spatial.hpp"

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

#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/check.hpp"
// IWYU pragma: end_keep

namespace pinocchio
{

  ///
  /// \brief Computes all the terms required to compute the derivatives of the placement, spatial
  /// velocity
  ///        for any joint of the model.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType Type of the joint velocity vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  /// \param[in] v The joint velocity (vector dim model.nv).
  ///
  /// \remarks This function is similar to do a forwardKinematics(model,data,q,v) followed by a
  /// computeJointJacobians(model,data,q).
  ///          In addition, it computes the spatial velocity of the joint expressed in the world
  ///          frame (see data.ov).
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType>
  void computeForwardKinematicsDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType> & v);

  ///
  /// \brief Computes all the terms required to compute the derivatives of the placement, spatial
  /// velocity and acceleration
  ///        for any joint of the model.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  /// \param[in] v The joint velocity (vector dim model.nv).
  /// \param[in] a The joint acceleration (vector dim model.nv).
  ///
  /// \remarks This function is similar to do a forwardKinematics(model,data,q,v) followed by a
  /// computeJointJacobians(model,data,q).
  ///          In addition, it computes the spatial velocity of the joint expressed in the world
  ///          frame (see data.ov).
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType,
    typename TangentVectorType1,
    typename TangentVectorType2>
  void computeForwardKinematicsDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q,
    const Eigen::MatrixBase<TangentVectorType1> & v,
    const Eigen::MatrixBase<TangentVectorType2> & a);

  ///
  /// \brief Computes the partial derivaties of the spatial velocity of a given with respect to
  ///        the joint configuration and velocity.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives with respect to the joint
  /// configuration vector.
  /// \tparam Matrix6xOut2 Matrix6x containing the partial derivatives with respect to the joint
  /// velocity vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] jointId Index of the joint in model.
  /// \param[in] rf Reference frame in which the Jacobian is expressed.
  /// \param[out] v_partial_dq Partial derivative of the joint velociy w.r.t. \f$ q \f$.
  /// \param[out] v_partial_dv Partial derivative of the joint velociy w.r.t. \f$ v \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix6xOut1,
    typename Matrix6xOut2>
  void getJointVelocityDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex jointId,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
    const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);

  ///
  /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to
  ///        the joint configuration, velocity and acceleration.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///        It is important to notice that a direct outcome (for free) of this algo is v_partial_dq
  ///        and v_partial_dv which is equal to a_partial_da.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with
  /// respect to the joint configuration vector.
  /// \tparam Matrix6xOut2 Matrix6x containing the
  /// partial derivatives of the spatial acceleration with respect to the joint configuration
  /// vector.
  /// \tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial
  /// acceleration with respect to the joint velocity vector.
  /// \tparam Matrix6xOut4 Matrix6x
  /// containing the partial derivatives of the spatial acceleration with respect to the joint
  /// acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] jointId Index of the joint in model.
  /// \param[in] rf Reference frame in which the Jacobian is expressed.
  /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q
  /// \f$.
  /// \param[out] a_partial_dv Partial derivative of the joint spatial acceleration w.r.t. \f$
  /// v \f$.
  /// \param[out] a_partial_da Partial derivative of the joint spatial acceleration w.r.t.
  /// \f$ \dot{v} \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix6xOut1,
    typename Matrix6xOut2,
    typename Matrix6xOut3,
    typename Matrix6xOut4>
  void getJointAccelerationDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex jointId,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
    const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
    const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
    const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);

  ///
  /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to
  ///        the joint configuration, velocity and acceleration.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///        It is important to notice that a direct outcome (for free) of this algo is v_partial_dq
  ///        and v_partial_dv which is equal to a_partial_da.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the spatial velocity with
  /// respect to the joint configuration vector.
  /// \tparam Matrix6xOut2 Matrix6x containing the
  /// partial derivatives of the spatial velocity with respect to the joint velocity vector.
  /// \tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint configuration vector.
  /// \tparam Matrix6xOut4 Matrix6x containing the
  /// partial derivatives of the spatial acceleration with respect to the joint velocity vector.
  /// \tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] jointId Index of the joint in model.
  /// \param[in] rf Reference frame in which the Jacobian is expressed.
  /// \param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
  /// \param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ v \f$.
  /// \param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q
  /// \f$.
  /// \param[out] a_partial_dv Partial derivative of the joint spatial acceleration w.r.t. \f$
  /// v \f$.
  /// \param[out] a_partial_da Partial derivative of the joint spatial acceleration w.r.t.
  /// \f$ \dot{v} \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix6xOut1,
    typename Matrix6xOut2,
    typename Matrix6xOut3,
    typename Matrix6xOut4,
    typename Matrix6xOut5>
  void getJointAccelerationDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex jointId,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
    const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
    const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
    const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
    const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);

  ///
  /// \brief Computes the partial derivatives of the velocity of a point given by its placement
  /// information w.r.t. the joint frame.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with
  /// respect to the joint configuration vector.
  /// \tparam Matrix3xOut2 Matrix3x containing the partial derivatives of the spatial velocity with
  /// respect to the joint velocity vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] placement Relative placement of the point w.r.t. the joint frame.
  /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or
  /// LOCAL_WORLD_ALIGNED).
  /// \param[out] v_point_partial_dq Partial derivative of the point velocity
  /// w.r.t. \f$ q \f$.
  /// \param[out] v_point_partial_dv Partial derivative of the point velociy
  /// w.r.t. \f$ v \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix3xOut1,
    typename Matrix3xOut2>
  void getPointVelocityDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const SE3Tpl<Scalar, Options> & placement,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
    const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv);

  ///
  /// \brief Computes the partial derivatives of the classic acceleration of a point given by its
  /// placement information w.r.t. the joint frame.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///        It is important to notice that a direct outcome (for free) of this algo is
  ///        v_point_partial_dq. v_point_partial_dv is not computed it is equal to
  ///        a_point_partial_da.
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with
  /// respect to the joint configuration vector.
  /// \tparam Matrix3xOut2 Matrix3x containing the
  /// partial derivatives of the spatial acceleration with respect to the joint configuration
  /// vector.
  /// \tparam Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial
  /// acceleration with respect to the joint velocity vector.
  /// \tparam Matrix3xOut4 Matrix3x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] placement Relative placement of the point w.r.t. the joint frame.
  /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or
  /// LOCAL_WORLD_ALIGNED).
  /// \param[out] v_point_partial_dq Partial derivative of the point velocity w.r.t. \f$ q \f$.
  /// \param[out] a_point_partial_dq Partial derivative of the point classic acceleration w.r.t. \f$
  /// q \f$.
  /// \param[out] a_point_partial_dv Partial derivative of the point classic acceleration w.r.t. \f$
  /// v \f$.
  /// \param[out] a_point_partial_da Partial derivative of the point classic acceleration w.r.t. \f$
  /// \dot{v} \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix3xOut1,
    typename Matrix3xOut2,
    typename Matrix3xOut3,
    typename Matrix3xOut4>
  void getPointClassicAccelerationDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const SE3Tpl<Scalar, Options> & placement,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
    const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
    const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
    const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da);

  ///
  /// \brief Computes the partial derivaties of the classic acceleration of a point given by its
  /// placement information w.r.t. to the joint frame.
  ///        You must first call computForwardKinematicsDerivatives before calling this function.
  ///        It is important to notice that a direct outcome (for free) of this algo is
  ///        v_point_partial_dq and v_point_partial_dv..
  ///
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam Matrix3xOut1 Matrix3x containing the partial derivatives of the spatial velocity with
  /// respect to the joint configuration vector.
  /// \tparam Matrix3xOut2 Matrix3x containing the
  /// partial derivatives of the spatial velocity with respect to the joint velocity vector.
  /// \tparam Matrix3xOut3 Matrix3x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint configuration vector.
  /// \tparam Matrix3xOut4 Matrix3x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint velocity vector.
  /// \tparam Matrix3xOut5 Matrix3x containing the partial derivatives of the spatial acceleration
  /// with respect to the joint acceleration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] placement Relative placement of the point w.r.t. the joint frame.
  /// \param[in] rf Reference frame in which the Jacobian is expressed (either LOCAL or
  /// LOCAL_WORLD_ALIGNED).
  /// \param[out] v_point_partial_dq Partial derivative of the point velocity w.r.t. \f$ q \f$.
  /// \param[out] v_point_partial_dv Partial derivative of the point velociy w.r.t. \f$ v \f$.
  /// \param[out] a_point_partial_dq Partial derivative of the point classic acceleration w.r.t. \f$
  /// q \f$.
  /// \param[out] a_point_partial_dv Partial derivative of the point classic acceleration w.r.t. \f$
  /// v \f$.
  /// \param[out] a_point_partial_da Partial derivative of the point classic acceleration w.r.t. \f$
  /// \dot{v} \f$.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename Matrix3xOut1,
    typename Matrix3xOut2,
    typename Matrix3xOut3,
    typename Matrix3xOut4,
    typename Matrix3xOut5>
  void getPointClassicAccelerationDerivatives(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const SE3Tpl<Scalar, Options> & placement,
    const ReferenceFrame rf,
    const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
    const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
    const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
    const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
    const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da);

  ///
  /// \brief Computes all the terms required to compute the second order derivatives of the
  /// placement information, also know as the
  ///        kinematic Hessian. This function assumes that the joint Jacobians (a.k.a data.J) has
  ///        been computed first. See \ref computeJointJacobians for such a function.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  ///
  /// \remarks This function is also related to \see getJointKinematicHessian.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void computeJointKinematicHessians(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data);

  ///
  /// \brief Computes all the terms required to compute the second order derivatives of the
  /// placement information, also know as the
  ///        kinematic Hessian.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration (vector dim model.nq).
  ///
  /// \remarks This function is also related to \see getJointKinematicHessian.
  ///
  template<
    typename Scalar,
    int Options,
    template<typename, int> class JointCollectionTpl,
    typename ConfigVectorType>
  void computeJointKinematicHessians(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    computeJointJacobians(model, data, q);
    computeJointKinematicHessians(model, data);
  }

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian. The frame in which the kinematic Hessian is precised by the input argument
  ///        rf.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed
  /// \param[out] kinematic_hessian Second order derivative of the joint placement w.r.t.
  /// \f$ q \f$ expressed in the frame given by rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians.
  /// kinematic_hessian has to be initialized with zero when calling this function
  ///          for the first time and there is no dynamic memory allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void getJointKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const ReferenceFrame rf,
    Tensor<Scalar, 3, Options> & kinematic_hessian);

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian. The frame in which the kinematic Hessian is precised by the input argument
  ///        rf.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] frame_placement   frame placement with respect to the parent joint
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed
  /// \param[out] kinematic_hessian Second order derivative of the joint placement w.r.t.
  /// \f$ q \f$ expressed in the frame given by rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians.
  /// kinematic_hessian has to be initialized with zero when calling this function
  ///          for the first time and there is no dynamic memory allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void getFrameKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const SE3Tpl<Scalar, Options> & frame_placement,
    const ReferenceFrame rf,
    Tensor<Scalar, 3, Options> & kinematic_hessian);

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian. The frame in which the kinematic Hessian is precised by the input argument
  ///        rf.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] frame_id Index of the frame in model.
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed
  /// \param[out] kinematic_hessian Second order derivative of the joint placement w.r.t.
  /// \f$ q \f$ expressed in the frame given by rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians.
  /// kinematic_hessian has to be initialized with zero when calling this function
  ///          for the first time and there is no dynamic memory allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  void getFrameKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::FrameIndex frame_id,
    const ReferenceFrame rf,
    Tensor<Scalar, 3, Options> & kinematic_hessian)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT(
      frame_id < (FrameIndex)model.nframes, "The index of the Frame is outside the bounds.");
    const auto frame = model.frames[frame_id];

    getFrameKinematicHessian(
      model, data, frame.parentJoint, frame.placement, rf, kinematic_hessian);
  }

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed.
  ///
  /// \returns The kinematic Hessian of the joint provided by its joint_id and expressed in the
  /// frame precised by the variable rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians. This function
  /// will proceed to some dynamic memory allocation for the return type.
  ///          Please refer to getJointKinematicHessian for a version without dynamic memory
  ///          allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  Tensor<Scalar, 3, Options> getJointKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const ReferenceFrame rf)
  {
    typedef Tensor<Scalar, 3, Options> ReturnType;
    ReturnType res(6, model.nv, model.nv);
    res.setZero();
    getJointKinematicHessian(model, data, joint_id, rf, res);
    return res;
  }

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian. The frame in which the kinematic Hessian is precised by the input argument
  ///        rf.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] joint_id Index of the joint in model.
  /// \param[in] frame_placement   frame placement with respect to the parent joint
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed
  /// \returns kinematic_hessian Second order derivative of the joint placement w.r.t.
  /// \f$ q \f$ expressed in the frame given by rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians. This function
  /// will proceed to some dynamic memory allocation for the return type.
  ///          Please refer to getFrameKinematicHessian for a version without dynamic memory
  ///          allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  Tensor<Scalar, 3, Options> getFrameKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::JointIndex joint_id,
    const SE3Tpl<Scalar, Options> & frame_placement,
    const ReferenceFrame rf)
  {
    typedef Tensor<Scalar, 3, Options> ReturnType;
    ReturnType res(6, model.nv, model.nv);
    res.setZero();
    getFrameKinematicHessian(model, data, joint_id, frame_placement, rf, res);
    return res;
  }

  ///
  /// \brief Retrieves the kinematic Hessian of a given joint according to the values aleardy
  /// computed by computeJointKinematicHessians
  ///        and stored in data. While the kinematic Jacobian of a given joint frame corresponds to
  ///        the first order derivative of the placement variation with respect to \f$ q \f$, the
  ///        kinematic Hessian corresponds to the second order derivation of placement variation,
  ///        which in turns also corresponds to the first order derivative of the kinematic
  ///        Jacobian. The frame in which the kinematic Hessian is precised by the input argument
  ///        rf.
  ///
  /// \tparam Scalar Scalar type of the kinematic model.
  /// \tparam Options Alignement options of the kinematic model.
  /// \tparam JointCollection Collection of Joint types.
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] frame_id Index of the frame in model.
  /// \param[in] rf Reference frame with respect to which the derivative of the Jacobian is
  /// expressed
  /// \returns Second order derivative of the joint placement w.r.t.
  /// \f$ q \f$ expressed in the frame given by rf.
  ///
  /// \remarks This function is also related to \see computeJointKinematicHessians. This function
  /// will proceed to some dynamic memory allocation for the return type.
  ///          Please refer to getFrameKinematicHessian for a version without dynamic memory
  ///          allocation.
  ///
  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
  Tensor<Scalar, 3, Options> getFrameKinematicHessian(
    const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
    const DataTpl<Scalar, Options, JointCollectionTpl> & data,
    const Model::FrameIndex frame_id,
    const ReferenceFrame rf)
  {
    typedef Tensor<Scalar, 3, Options> ReturnType;
    ReturnType res(6, model.nv, model.nv);
    res.setZero();
    getFrameKinematicHessian(model, data, frame_id, rf, res);
    return res;
  }

} // namespace pinocchio

// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/kinematics-derivatives.hxx"
// IWYU pragma: end_exports
