Unverified Commit a964af51 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1172 from proyan/devel

dIntegrateTransport algorithm.
parents a8eba917 3b53536a
//
// Copyright (c) 2016-2019 CNRS INRIA
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_joint_configuration_hpp__
#define __pinocchio_joint_configuration_hpp__
#ifndef __pinocchio_algorithm_joint_configuration_hpp__
#define __pinocchio_algorithm_joint_configuration_hpp__
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
namespace pinocchio
......@@ -345,7 +343,121 @@ namespace pinocchio
{
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
}
/**
*
* @brief Transport a matrix from the terminal to the originate 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 as vectorial 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>
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 originate 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 as vectorial 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 originate 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 as vectorial 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>
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 originate 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 as vectorial 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.
......@@ -853,5 +965,4 @@ namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/joint-configuration.hxx"
#endif // ifndef __pinocchio_joint_configuration_hpp__
#endif // ifndef __pinocchio_algorithm_joint_configuration_hpp__
//
// Copyright (c) 2016-2019 CNRS INRIA
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_joint_configuration_hxx__
#define __pinocchio_joint_configuration_hxx__
#ifndef __pinocchio_algorithm_joint_configuration_hxx__
#define __pinocchio_algorithm_joint_configuration_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
......@@ -171,6 +171,54 @@ namespace pinocchio
}
}
template<typename LieGroup_t, 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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == model.nv, "The input matrix is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == Jin.rows(), "The output argument should be the same size as input matrix");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == Jin.cols(), "The output argument should be the same size as input matrix");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateTransportStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType2,Jout),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
}
}
template<typename LieGroup_t, 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)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == model.nv, "The input matrix is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
}
}
template<typename LieGroup_t, 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,
......@@ -373,5 +421,4 @@ namespace pinocchio
} // namespace pinocchio
#endif // ifndef __pinocchio_joint_configuration_hxx__
#endif // ifndef __pinocchio_algorithm_joint_configuration_hxx__
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2020 CNRS CNRS
//
#ifndef __pinocchio_cartesian_product_operation_hpp__
#define __pinocchio_cartesian_product_operation_hpp__
#ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
#define __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
#include <pinocchio/multibody/liegroup/liegroup-base.hpp>
......@@ -175,9 +175,52 @@ namespace pinocchio
default:
assert(false && "Wrong Op requesed value");
break;
}
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
lg1_.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
lg2_.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in);
lg1_.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>());
lg2_.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>());
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & Jin) const
{
Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
lg1_.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
lg2_.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & Jin) const
{
Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin);
lg1_.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>());
lg2_.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>());
}
template <class ConfigL_t, class ConfigR_t>
......@@ -252,4 +295,4 @@ namespace pinocchio
} // namespace pinocchio
#endif // ifndef __pinocchio_cartesian_product_operation_hpp__
#endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018-2020 CNRS INRIA
//
#ifndef __pinocchio_lie_group_algo_hpp__
#define __pinocchio_lie_group_algo_hpp__
#ifndef __pinocchio_multibody_liegroup_liegroup_algo_hpp__
#define __pinocchio_multibody_liegroup_liegroup_algo_hpp__
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
/// Details
#include "pinocchio/multibody/liegroup/liegroup-algo.hxx"
#endif // ifndef __pinocchio_lie_group_algo_hpp__
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_algo_hpp__
//
// Copyright (c) 2018-2019 CNRS INRIA
// Copyright (c) 2018-2020 CNRS INRIA
//
#ifndef __pinocchio_lie_group_algo_hxx__
#define __pinocchio_lie_group_algo_hxx__
#ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__
#define __pinocchio_multibody_liegroup_liegroup_algo_hxx__
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
......@@ -191,6 +191,85 @@ namespace pinocchio
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixInType, typename JacobianMatrixOutType>
struct dIntegrateTransportStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixInType,JacobianMatrixOutType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
const JacobianMatrixInType &,
JacobianMatrixOutType &,
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateTransportStepAlgo, dIntegrateTransportStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateTransportStepAlgo
{
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixInType, typename JacobianMatrixOutType>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixInType> & mat_in,
const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out,
const ArgumentPosition & arg)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointRows(mat_in.derived()),
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateTransportInPlaceStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
struct dIntegrateTransportInPlaceStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixType &,
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportInPlaceStepAlgo,
dIntegrateTransportInPlaceStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateTransportInPlaceStepAlgo
{
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixType> & mat,
const ArgumentPosition & arg)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportInPlaceStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
......@@ -523,4 +602,4 @@ namespace pinocchio
}
#endif // ifndef __pinocchio_lie_group_algo_hxx__
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_algo_hxx__
//
// Copyright (c) 2016-2019 CNRS INRIA
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_lie_group_operation_base_hpp__
#define __pinocchio_lie_group_operation_base_hpp__
#ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
#define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
#include "pinocchio/multibody/liegroup/fwd.hpp"
......@@ -154,6 +154,150 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const;
/**
*
* @brief Transport a matrix from the terminal to the originate 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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
* @param[in] arg argument with respect to which the differentiation is performed (ARG0 corresponding to q, and ARG1 to v)
*
* @param[out] Jout Transported matrix
*
*/
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg) const;
/**
*
* @brief Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration argument.
*
* @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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
*
* @brief Transport a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the velocity argument.
*
* @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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
*
* @brief Transport in place a matrix from the terminal to the originate 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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in,out] J the input matrix
* @param[in] arg argument with respect to which the differentiation is performed (ARG0 corresponding to q, and ARG1 to v)
*/
template<class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J,
const ArgumentPosition arg) const;
/**
*
* @brief Transport in place a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the configuration argument.
*
* @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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
*
* @brief Transport in place a matrix from the terminal to the originate tangent space of the integrate operation, with respect to the velocity argument.
*
* @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 as vectorial spaces, this operation corresponds to Identity.
* For Lie groups, its corresponds to the canonical vector field transportation.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
* @brief Interpolation between two joint's configurations
*
......@@ -386,4 +530,4 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
#include "pinocchio/multibody/liegroup/liegroup-base.hxx"
#endif // ifndef __pinocchio_lie_group_operation_base_hpp__
#endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__