pinocchio Namespace Reference

Main pinocchio namespace. More...

Namespaces
	buildModels
	Cholesky 分解
	cholesky
	专用容器
	forceSet
	构建简单模型
	motionSet
	力作用集合
	quaternion
	运动作用集合
	rpy
	四元数运算
	srdf
	URDF 解析
	urdf
	横滚-俯仰-偏航运算

Classes
struct	ABAChecker
struct	ADMMConstraintSolverTpl
	ADMM constraint solver. More...
struct	ADMMSolverResultTpl
	Struct describing the solution of the ADMM constraint solver after calling the solve method. Also contains the warmstart of the solution to the constraint problem. More...
struct	ADMMSolverSettingsTpl
	Settings for the ADMM constraint solver loop. More...
struct	ADMMSolverStatsTpl
	Struct to track per iteration progress of ADMM constraint solver. More...
struct	AlgorithmCheckerList
struct	BroadPhaseManagerBase
struct	CRBAChecker
struct	is_lie_group_map
	Helpers to use SFINAE for safe candidate selection compare to liegroup-variant-visitors.hxx. More...
struct	is_lie_group_map< T, std::void_t< typename T::template operation< void > * > >
struct	MimicChecker
	Simple model checker, that assert that there is a mimic joint in the tree. More...
struct	ParentChecker
	Simple model checker, that assert that model.parents is indeed a tree. More...
struct	PGSConstraintSolverTpl
	Projected Gauss Siedel solver. More...
struct	PGSSolverResultTpl
	Struct describing the solution of the PGS constraint solver after calling the solve method. Also contains the warmstart of the solution to the constraint problem. More...
struct	PGSSolverSettingsTpl
	Settings for the PGS constraint solver loop. More...
struct	PGSSolverStatsTpl
	Struct to track per iteration progress of PGS constraint solver. More...
struct	ReachableSetParams
	Parameters for the reachable space algorithm. More...
struct	ReachableSetResults
	Structure containing the return value for the reachable algorithm. More...
struct	traits< ADMMConstraintSolverTpl< _Scalar, _Options > >
struct	traits< ADMMSolverResultTpl< _Scalar, _Options > >
struct	traits< ADMMSolverSettingsTpl< _Scalar > >
struct	traits< ADMMSolverStatsTpl< _Scalar > >
struct	traits< PGSConstraintSolverTpl< _Scalar, _Options > >
struct	traits< PGSSolverResultTpl< _Scalar, _Options > >
struct	traits< PGSSolverSettingsTpl< _Scalar > >
struct	traits< PGSSolverStatsTpl< _Scalar > >

Typedefs
typedef ADMMConstraintSolverTpl< context::Scalar, context::Options >	ADMMConstraintSolver
typedef ADMMSolverResultTpl< context::Scalar, context::Options >	ADMMSolverResult
typedef ADMMSolverSettingsTpl< context::Scalar >	ADMMSolverSettings
typedef ADMMSolverStatsTpl< context::Scalar >	ADMMSolverStats
typedef PGSConstraintSolverTpl< context::Scalar, context::Options >	PGSConstraintSolver
typedef PGSSolverResultTpl< context::Scalar, context::Options >	PGSSolverResult
typedef PGSSolverSettingsTpl< context::Scalar >	PGSSolverSettings
typedef PGSSolverStatsTpl< context::Scalar >	PGSSolverStats

Enumerations
enum class	ADMMProximalRule : char { MANUAL = 'M' , AUTOMATIC = 'A' }
	ADMM proximal policy. MANUAL: mu_prox is constant and manually set. It is scaled by tau_prox AUTOMATIC: mu_prox is always set to rho. More...
enum class	ADMMUpdateRule : char { SPECTRAL = 'S' , OSQP = 'O' , LINEAR = 'L' , CONSTANT = 'C' }
	ADMM rho update rule. SPECTRAL: if primal/dual ratio met, multiply rho by a spectral factor. OSQP: if primal/dual ratio met, multiply rho by sqrt(primal_feas/dual_feas). LINEAR: if primal/dual ratio met, multiply rho by a linear factor. CONSTANT: keep the same rho.

Functions
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	aba (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 > &tau, const Convention convention=Convention::LOCAL)
	The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename SpatialForce , typename SpatialForceAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	aba (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 > &tau, const std::vector< SpatialForce, SpatialForceAllocator > &fext, const Convention convention=Convention::LOCAL)
	The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model and the external forces. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorPool , typename TangentVectorPool1 , typename TangentVectorPool2 , typename TangentVectorPool3 >
void	abaInParallel (const size_t num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &tau, const Eigen::MatrixBase< TangentVectorPool3 > &a)
	A parallel version of the Articulated Body algorithm. It computes the forward dynamics, aka the joint acceleration according to the current state of the system and the desired joint torque. More...
void	appendGeometryModel (GeometryModel &geom_model1, const GeometryModel &geom_model2)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
ModelTpl< Scalar, Options, JointCollectionTpl >	appendModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &modelA, const ModelTpl< Scalar, Options, JointCollectionTpl > &modelB, const FrameIndex frameInModelA, const SE3Tpl< Scalar, Options > &aMb)
	Append a child model into a parent model, after a specific frame given by its index. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	appendModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &modelA, const ModelTpl< Scalar, Options, JointCollectionTpl > &modelB, const FrameIndex frameInModelA, const SE3Tpl< Scalar, Options > &aMb, ModelTpl< Scalar, Options, JointCollectionTpl > &model)
	Append a child model into a parent model, after a specific frame given by its index. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	appendModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &modelA, const ModelTpl< Scalar, Options, JointCollectionTpl > &modelB, const GeometryModel &geomModelA, const GeometryModel &geomModelB, const FrameIndex frameInModelA, const SE3Tpl< Scalar, Options > &aMb, ModelTpl< Scalar, Options, JointCollectionTpl > &model, GeometryModel &geomModel)
	Append a child model into a parent model, after a specific frame given by its index. More...
template<typename MotionVelocity , typename MotionAcceleration >
Eigen::Matrix< typename MotionVelocity::Scalar, 6, 10, typename MotionVelocity::Vector3 ::Options >	bodyRegressor (const MotionDense< MotionVelocity > &v, const MotionDense< MotionAcceleration > &a)
	Computes the regressor for the dynamic parameters of a single rigid body. More...
template<typename MotionVelocity , typename MotionAcceleration , typename OutputType >
void	bodyRegressor (const MotionDense< MotionVelocity > &v, const MotionDense< MotionAcceleration > &a, const Eigen::MatrixBase< OutputType > &regressor)
	Computes the regressor for the dynamic parameters of a single rigid body. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	buildMimicModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &input_model, const std::vector< JointIndex > &index_mimicked, const std::vector< JointIndex > &index_mimicking, const std::vector< Scalar > &scaling, const std::vector< Scalar > &offset, ModelTpl< Scalar, Options, JointCollectionTpl > &output_model)
	Transform joints of a model into mimic joints. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	buildReducedModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, const std::vector< JointIndex > &list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model, GeometryModel &reduced_geom_model)
	Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	buildReducedModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::vector< GeometryModel > &list_of_geom_models, const std::vector< JointIndex > &list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model, std::vector< GeometryModel > &list_of_reduced_geom_models)
	Build a reduced model and a reduced geometry model from a given input model, a given input geometry model and a list of joint to lock. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
ModelTpl< Scalar, Options, JointCollectionTpl >	buildReducedModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::vector< JointIndex > &list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration)
	Build a reduced model from a given input model and a list of joint to lock. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	buildReducedModel (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, std::vector< JointIndex > list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model)
	Build a reduced model from a given input model and a list of joint to lock. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	ccrba (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta according to the current joint configuration and velocity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	centerOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const bool computeSubtreeComs=true)
	Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data. The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	centerOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
	Computes the center of mass position of a given model according to a particular joint configuration. The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	centerOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const bool computeSubtreeComs=true)
	Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity. The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	centerOfMass (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, const bool computeSubtreeComs=true)
	Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration. The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation. And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	centerOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, KinematicLevel kinematic_level, const bool computeSubtreeComs=true)
	Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the requested kinematic_level. The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
bool	checkData (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	computeABADerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	The derivatives of the Articulated-Body algorithm. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
std::enable_if_t< ConfigVectorType::IsVectorAtCompileTime||TangentVectorType1::IsVectorAtCompileTime||TangentVectorType2::IsVectorAtCompileTime, void >	computeABADerivatives (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 > &tau)
	The derivatives of the Articulated-Body algorithm. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
void	computeABADerivatives (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 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
	The derivatives of the Articulated-Body algorithm. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename SpatialForce , typename SpatialForceAllocator >
void	computeABADerivatives (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 > &tau, const std::vector< SpatialForce, SpatialForceAllocator > &fext)
	The derivatives of the Articulated-Body algorithm with external forces. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename SpatialForce , typename SpatialForceAllocator , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
void	computeABADerivatives (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 > &tau, const std::vector< SpatialForce, SpatialForceAllocator > &fext, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
	The derivatives of the Articulated-Body algorithm with external forces. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
std::enable_if_t< !(MatrixType1::IsVectorAtCompileTime||MatrixType2::IsVectorAtCompileTime||MatrixType3::IsVectorAtCompileTime), void >	computeABADerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
	The derivatives of the Articulated-Body algorithm. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename SpatialForce , typename SpatialForceAllocator >
void	computeABADerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< SpatialForce, SpatialForceAllocator > &fext)
	The derivatives of the Articulated-Body algorithm with external forces. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename SpatialForce , typename SpatialForceAllocator , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
void	computeABADerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< SpatialForce, SpatialForceAllocator > &fext, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
	The derivatives of the Articulated-Body algorithm with external forces. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
void	computeAllTerms (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the same time to: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	computeBodyRadius (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, GeometryData &geom_data)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename Matrix6xLike0 , typename Matrix6xLike1 , typename Matrix6xLike2 , typename Matrix6xLike3 >
void	computeCentroidalDynamicsDerivatives (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, const Eigen::MatrixBase< Matrix6xLike0 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
	Computes the analytical derivatives of the centroidal dynamics with respect to the joint configuration vector, velocity and acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	computeCentroidalMap (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the Centroidal Momentum Matrix. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	computeCentroidalMapTimeVariation (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the Centroidal Momentum Matrix time derivative. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Force &	computeCentroidalMomentum (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center of mass. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Force &	computeCentroidalMomentum (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center of mass. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Force &	computeCentroidalMomentumTimeVariation (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative expressed around the center of mass. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
const DataTpl< Scalar, Options, JointCollectionTpl >::Force &	computeCentroidalMomentumTimeVariation (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)
	Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative expressed around the center of mass. More...
bool	computeCollision (const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id)
	Compute the collision status between a SINGLE collision pair. The result is store in the collisionResults vector. More...
bool	computeCollision (const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id, coal::CollisionRequest &collision_request)
	Compute the collision status between a SINGLE collision pair. The result is store in the collisionResults vector. More...
template<typename BroadPhaseManagerDerived >
bool	computeCollisions (BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback)
	Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements and broadphase_manager.update() have been called first. More...
template<typename BroadPhaseManagerDerived >
bool	computeCollisions (BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, const bool stopAtFirstCollision=false)
	Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements and broadphase_manager.update() have been called first. More...
bool	computeCollisions (const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
	Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements has been called first. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename BroadPhaseManagerDerived , typename ConfigVectorType >
bool	computeCollisions (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback, const Eigen::MatrixBase< ConfigVectorType > &q)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename BroadPhaseManagerDerived , typename ConfigVectorType >
bool	computeCollisions (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, const Eigen::MatrixBase< ConfigVectorType > &q, const bool stopAtFirstCollision=false)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
bool	computeCollisions (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool stopAtFirstCollision=false)
template<typename BroadPhaseManagerDerived , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorPool , typename CollisionVectorResult >
void	computeCollisionsInParallel (const size_t num_threads, BroadPhaseManagerPoolBase< BroadPhaseManagerDerived, Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< CollisionVectorResult > &res, const bool stopAtFirstCollisionInConfiguration=false, const bool stopAtFirstCollisionInBatch=false)
template<typename BroadPhaseManagerDerived , typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	computeCollisionsInParallel (const size_t num_threads, BroadPhaseManagerPoolBase< BroadPhaseManagerDerived, Scalar, Options, JointCollectionTpl > &pool, const std::vector< Eigen::MatrixXd > &trajectories, std::vector< VectorXb > &res, const bool stopAtFirstCollisionInTrajectory=false)
	Evaluate the collision over a set of trajectories and return whether a trajectory contains a collision.
bool	computeCollisionsInParallel (const size_t num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
bool	computeCollisionsInParallel (const size_t num_threads, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool stopAtFirstCollision=false)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorPool , typename CollisionVectorResult >
void	computeCollisionsInParallel (const size_t num_threads, GeometryPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< CollisionVectorResult > &res, const bool stopAtFirstCollisionInConfiguration=false, const bool stopAtFirstCollisionInBatch=false)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator >
void	computeConstraintDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 , typename MatrixType4 , typename MatrixType5 , typename MatrixType6 >
void	computeConstraintDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator >
void	computeConstraintDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 , typename MatrixType4 , typename MatrixType5 , typename MatrixType6 >
void	computeConstraintDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
void	computeContactPatch (const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id)
	Compute the contact patch info associated with the collision pair given by pair_id. Note that an actual computation will only occur if the collision pair is indeed in collision (i.e. only if geom_data.collisionResult[pair_id].isCollision() is true). More...
void	computeContactPatches (const GeometryModel &geom_model, GeometryData &geom_data)
	Calls computeContactPatch for every collision pair. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs &	computeCoriolisMatrix (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the Coriolis Matrix \( C(q,\dot{q}) \) of the Lagrangian dynamics: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , class ModelAllocator , class DataAllocator , typename MatrixType >
void	computeDampedDelassusMatrixInverse (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data, const Eigen::MatrixBase< MatrixType > &damped_delassus_inverse, const Scalar mu, const bool scaled=false, const bool Pv=true)
	Computes the inverse of the Delassus matrix associated to a set of given constraints. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , class ModelAllocator , class DataAllocator , typename MatrixType >
void	computeDelassusMatrix (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data, const Eigen::MatrixBase< MatrixType > &delassus, const Scalar mu=0)
	Computes the Delassus matrix associated to a set of given constraints. More...
coal::DistanceResult &	computeDistance (const GeometryModel &geom_model, GeometryData &geom_data, const PairIndex pair_id)
	Compute the minimal distance between collision objects of a SINGLE collison pair. More...
std::size_t	computeDistances (const GeometryModel &geom_model, GeometryData &geom_data)
	Compute the minimal distance between collision objects of a ALL collison pair. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
std::size_t	computeDistances (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
std::size_t	computeDistances (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
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)
	Computes all the terms required to compute the derivatives of the placement, spatial velocity for any joint of the model. More...
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)
	Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration for any joint of the model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename Matrix6xLike >
void	computeFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frame_id, const Eigen::MatrixBase< Matrix6xLike > &J)
	Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate system. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename Matrix6xLike >
void	computeFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frame_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
	Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x	computeFrameKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf)
	Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the frame given as input. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xReturnType >
void	computeFrameKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	computeGeneralizedGravity (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the generalized gravity contribution \( g(q) \) of the Lagrangian dynamics: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename ReturnMatrixType >
void	computeGeneralizedGravityDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ReturnMatrixType > &gravity_partial_dq)
	Computes the partial derivative of the generalized gravity contribution with respect to the joint configuration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator >
void	computeImpulseDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const Scalar r_coeff, const ProximalSettingsTpl< Scalar > &settings)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModelAllocator , class ConstraintDataAllocator , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 , typename MatrixType4 >
void	computeImpulseDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const Scalar r_coeff, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &dvimpulse_partial_dq, const Eigen::MatrixBase< MatrixType2 > &dvimpulse_partial_dv, const Eigen::MatrixBase< MatrixType3 > &impulse_partial_dq, const Eigen::MatrixBase< MatrixType4 > &impulse_partial_dv)
template<typename Scalar , class PointContactConstraintModelVector , class PointContactConstraintDataVector , typename VectorLikeC , typename VectorLikeResult >
bool	computeInverseDynamicsConstraintForces (const PointContactConstraintModelVector &constraint_models, const PointContactConstraintDataVector &constraint_datas, const Eigen::MatrixBase< VectorLikeC > &c_ref, const Eigen::MatrixBase< VectorLikeResult > &_lambda, ProximalSettingsTpl< Scalar > &settings, bool solve_ncp=true)
	Compute the contact forces given a target velocity of contact points. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename Matrix6Like >
void	computeJointJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex joint_id, const Eigen::MatrixBase< Matrix6Like > &J)
	Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store the result in the input argument J. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	computeJointJacobians (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame. The result is accessible through data.J. This function assumes that pinocchio::forwardKinematics has been called before. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	computeJointJacobians (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame. The result is accessible through data.J. This function computes also the forwardKinematics of the model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	computeJointJacobiansTimeVariation (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v. The result is accessible through data.dJ. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	computeJointKinematicHessians (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	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 computeJointJacobians for such a function. More...
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)
	Computes all the terms required to compute the second order derivatives of the placement information, also know as the kinematic Hessian. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x	computeJointKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame rf)
	Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the joint given as input. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xReturnType >
void	computeJointKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x	computeJointKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame rf, const SE3Tpl< Scalar, Options > &placement)
	Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xReturnType >
void	computeJointKinematicRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame rf, const SE3Tpl< Scalar, Options > &placement, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs &	computeJointTorqueRegressor (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)
	Computes the joint torque regressor that links the joint torque to the dynamic parameters of each link according to the current the robot motion. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Scalar	computeKineticEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
Scalar	computeKineticEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename ConstraintMatrixType , typename KKTMatrixType >
void	computeKKTContactDynamicMatrixInverse (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv, const Scalar &inv_damping=0.)
	Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the following matrix: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Scalar	computeMechanicalEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the mechanical energy of the system stored in data.mechanical_energy. The result is accessible through data.kinetic_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
Scalar	computeMechanicalEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the mechanical energy of the system stored in data.mechanical_energy. The result is accessible through data.kinetic_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs &	computeMinverse (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the inverse of the joint space inertia matrix using Articulated Body formulation. Compared to the complete signature computeMinverse<Scalar,Options,ConfigVectorType>, this version assumes that ABA has been called first. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs &	computeMinverse (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the inverse of the joint space inertia matrix using Articulated Body formulation. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Scalar	computePotentialEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Computes the potential energy of the system, i.e. the potential energy linked to the gravity field. The result is accessible through data.potential_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
Scalar	computePotentialEnergy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the potential energy of the system, i.e. the potential energy linked to the gravity field. The result is accessible through data.potential_energy. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::RowVectorXs &	computePotentialEnergyRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
void	computeRNEADerivatives (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)
	Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
void	computeRNEADerivatives (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, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
	Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
void	computeRNEADerivatives (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, const std::vector< ForceTpl< Scalar, Options >> &fext)
	Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename MatrixType1 , typename MatrixType2 , typename MatrixType3 >
void	computeRNEADerivatives (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, const std::vector< ForceTpl< Scalar, Options >> &fext, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
	Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
void	ComputeRNEASecondOrderDerivatives (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)
	Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename Tensor1 , typename Tensor2 , typename Tensor3 , typename Tensor4 >
void	ComputeRNEASecondOrderDerivatives (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, const Tensor1 &d2tau_dqdq, const Tensor2 &d2tau_dvdv, const Tensor3 &dtau_dqdv, const Tensor4 &dtau_dadq)
	Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithm w.r.t the joint configuration, the joint velocity and the joint acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x &	computeStaticRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Computes the static regressor that links the center of mass positions of all the links to the center of mass of the complete model according to the current configuration of the robot. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	computeStaticTorque (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< ForceTpl< Scalar, Options >> &fext)
	Computes the generalized static torque contribution \( g(q) - \sum J(q)^{\top} f_{\text{ext}} \) of the Lagrangian dynamics: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename ReturnMatrixType >
void	computeStaticTorqueDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< ForceTpl< Scalar, Options >> &fext, const Eigen::MatrixBase< ReturnMatrixType > &static_torque_partial_dq)
	Computes the partial derivative of the generalized gravity and external forces contributions (a.k.a static torque vector) with respect to the joint configuration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	computeSubtreeMasses (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds to the total mass of the model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
ForceTpl< Scalar, Options >	computeSupportedForceByFrame (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
	Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame. The supported force corresponds to the sum of all the forces experienced after the given frame, i.e : More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
InertiaTpl< Scalar, Options >	computeSupportedInertiaByFrame (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, bool with_subtree)
	Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame. The total supported inertia corresponds to the sum of all the inertia after the given frame, i.e : More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Scalar	computeTotalMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
	Compute the total mass of the model and return it. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Scalar	computeTotalMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Compute the total mass of the model, put it in data.mass[0] and return it. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ConstraintModel , class ConstraintModelAllocator , class ConstraintData , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	constrainedABA (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 > &tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &contact_models, std::vector< ConstraintData, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
	The constrained Articulated Body Algorithm (constrainedABA). It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system. All the quantities are expressed in the LOCAL coordinate systems of the joint frames. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ConstraintModelAllocator , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	constraintDynamics (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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas)
	Computes the forward dynamics with contact constraints according to a given list of contact information. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ConstraintModelAllocator , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	constraintDynamics (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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
	Computes the forward dynamics with contact constraints according to a given list of contact information. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ModelAllocator , class DataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	contactABA (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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class PointContactConstraintModelVector , class PointContactConstraintDataVector , typename VectorLikeGamma , typename VectorLikeLam >
bool	contactInverseDynamics (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, const Scalar dt, const PointContactConstraintModelVector &constraint_models, PointContactConstraintDataVector &constraint_datas, const Eigen::MatrixBase< VectorLikeGamma > &constraint_correction, const Eigen::MatrixBase< VectorLikeLam > &_lambda_sol, ProximalSettingsTpl< Scalar > &settings, bool solve_ncp=true)
	The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the presence of contacts, aka the joint torques according to the current state of the system and the desired joint accelerations. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	copy (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
	Copy part of the data from origin to dest. Template parameter can be used to select at which differential level the copy should occur. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs &	crba (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
	Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008). The result is accessible through data.M. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x &	dccrba (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
JointIndex	findCommonAncestor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, JointIndex joint1_id, JointIndex joint2_id)
	Computes the common ancestor between two joints belonging to the same kinematic tree. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
JointIndex	findCommonAncestor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, JointIndex joint1_id, JointIndex joint2_id, size_t &index_ancestor_in_support1, size_t &index_ancestor_in_support2)
	Computes the common ancestor between two joints belonging to the same kinematic tree. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename ConstraintMatrixType , typename DriftVectorType >
PINOCCHIO_UNSUPPORTED const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	forwardDynamics (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 > &tau, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
	Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is called. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename TangentVectorType , typename ConstraintMatrixType , typename DriftVectorType >
PINOCCHIO_UNSUPPORTED const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	forwardDynamics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< TangentVectorType > &tau, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
	Compute the forward dynamics with contact constraints, assuming pinocchio::computeAllTerms has been called. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	forwardKinematics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Update the joint placements according to the current joint configuration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
void	forwardKinematics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Update the joint placements and spatial velocities according to the current joint configuration and velocity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
void	forwardKinematics (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)
	Update the joint placements, spatial velocities and spatial accelerations according to the current joint configuration, velocity and acceleration. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType &	frameBodyRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, FrameIndex frame_id)
	Computes the regressor for the dynamic parameters of a rigid body attached to a given frame, puts the result in data.bodyRegressor and returns it. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	framesForwardKinematics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
	First calls the forwardKinematics on the model, then computes the placement of each frame. /sa pinocchio::forwardKinematics. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
	Returns the spatial acceleration of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix3xOut >
void	getCenterOfMassVelocityDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Matrix3xOut > &vcom_partial_dq)
	Computes the partial derivatie of the center-of-mass velocity with respect to the joint configuration q. You must first call computeAllTerms(model,data,q,v) or computeCenterOfMass(model,data,q,v) before calling this function. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xLike0 , typename Matrix6xLike1 , typename Matrix6xLike2 , typename Matrix6xLike3 >
void	getCentroidalDynamicsDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Matrix6xLike1 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
	Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives. pinocchio::computeRNEADerivatives should have been called first. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getClassicalAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
	Returns the "classical" acceleration of the joint expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 &	getComFromCrba (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix). More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs &	getCoriolisMatrix (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Retrives the Coriolis Matrix \( C(q,\dot{q}) \) of the Lagrangian dynamics: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
	Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in the data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
	Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 , typename Matrix6xOut3 , typename Matrix6xOut4 >
void	getFrameAccelerationDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, 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)
	Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 , typename Matrix6xOut3 , typename Matrix6xOut4 , typename Matrix6xOut5 >
void	getFrameAccelerationDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, 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)
	Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 , typename Matrix6xOut3 , typename Matrix6xOut4 >
void	getFrameAccelerationDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, 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)
	Computes the partial derivatives of the spatial acceleration of a frame given by its relative placement, with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 , typename Matrix6xOut3 , typename Matrix6xOut4 , typename Matrix6xOut5 >
void	getFrameAccelerationDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, 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)
	Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameClassicalAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
	Returns the "classical" acceleration of the Frame expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameClassicalAcceleration (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
	Returns the "classical" acceleration of the Frame expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options >	getFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame reference_frame)
	Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xLike >
void	getFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
	Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options >	getFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame)
	Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xLike >
void	getFrameJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
	Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xLike >
void	getFrameJacobianTimeVariation (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &dJ)
	Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the WORLD frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the LOCAL frame (rf = LOCAL). More...
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)
	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 \( q \), 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. More...
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)
	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 \( q \), 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. More...
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)
	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 \( q \), 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. More...
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)
	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 \( q \), 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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameVelocity (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
	Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getFrameVelocity (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
	Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 >
void	getFrameVelocityDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
	Computes the partial derivatives of the spatial velocity of a frame given by its relative placement, with respect to q and v. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6xOut1 , typename Matrix6xOut2 >
void	getFrameVelocityDerivatives (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
	Computes the partial derivatives of the frame spatial velocity with respect to q and v. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x &	getJacobianComFromCrba (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM position from the joint space inertia matrix (also called the mass matrix). The results are accessible through data.Jcom, data.mass[0] and data.com[0] and are both expressed in the world frame. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix3xLike >
void	getJacobianSubtreeCenterOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
	Retrieves the Jacobian of the center of mass of the given subtree according to the current value stored in data. It assumes that pinocchio::jacobianCenterOfMass has been called first with computeSubtreeComs equals to true. More...
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)
	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. More...
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)
	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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options >	getJointJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame)
	Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6Like >
void	getJointJacobian (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
	Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame options. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix6Like >
void	getJointJacobianTimeVariation (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &dJ)
	Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint. More...
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)
	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 \( q \), 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. More...
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)
	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 \( q \), 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. More...
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)
	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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConstraintMatrixType , typename KKTMatrixType >
PINOCCHIO_UNSUPPORTED void	getKKTContactDynamicMatrixInverse (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
	Computes the inverse of the KKT matrix for dynamics with contact constraints. More...
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)
	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. More...
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)
	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.. More...
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)
	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. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
SE3Tpl< Scalar, Options >	getRelativePlacement (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointIdRef, const JointIndex jointIdTarget, const Convention convention=Convention::LOCAL)
	Returns the relative placement of two joints expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement values in data structure. LOCAL convention should only be used when aba and crba algorithms are called in LOCAL convention as well. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
MotionTpl< Scalar, Options >	getVelocity (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
	Returns the spatial velocity of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename ConstraintMatrixType >
PINOCCHIO_UNSUPPORTED const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	impulseDynamics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
	Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , class ConstraintModelAllocator , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	impulseDynamics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v_before, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, const Scalar r_coeff, const ProximalSettingsTpl< Scalar > &settings)
	Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename ConstraintMatrixType >
PINOCCHIO_UNSUPPORTED const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	impulseDynamics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
	Compute the impulse dynamics with contact constraints, assuming pinocchio::crba has been called. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModel , class ConstraintModelAllocator , class ConstraintData , class ConstraintDataAllocator >
void	initConstraintDynamics (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< ConstraintModel, ConstraintModelAllocator > &contact_models, const std::vector< ConstraintData, ConstraintDataAllocator > &contact_datas)
	Init the forward dynamics data according to the contact information contained in contact_models. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class Allocator >
void	initPvDelassus (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, class ConstraintModel , class ConstraintModelAllocator >
void	initPvSolver (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< ConstraintModel, ConstraintModelAllocator > &contact_models)
	Init the data according to the contact information contained in contact_models. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x &	jacobianCenterOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const bool computeSubtreeComs=true)
	Computes both the jacobian and the the center of mass position of a given model according to the current value stored in data. It assumes that forwardKinematics has been called first. The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x &	jacobianCenterOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
	Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration. The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename Matrix3xLike >
void	jacobianSubtreeCenterOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
	Computes the Jacobian of the center of mass of the given subtree according to a particular joint configuration. In addition, the algorithm also computes the Jacobian of all the joints (. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename Matrix3xLike >
void	jacobianSubtreeCenterOfMass (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex &rootSubtreeId, const Eigen::MatrixBase< Matrix3xLike > &res)
	Computes the Jacobian of the center of mass of the given subtree according to the current value stored in data. It assumes that forwardKinematics has been called first. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType &	jointBodyRegressor (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, JointIndex joint_id)
	Computes the regressor for the dynamic parameters of a rigid body attached to a given joint, puts the result in data.bodyRegressor and returns it. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ConstraintModel , class ConstraintModelAllocator , class ConstraintData , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	lcaba (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 > &tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &constraint_models, std::vector< ConstraintData, ConstraintDataAllocator > &constraint_datas, ProximalSettingsTpl< Scalar > &settings)
	The closed-loop constrained Articulated Body Algorithm (CLconstrainedABA). It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system for mechanisms potentially with internal closed kinematic loops. More...
template<class... T>
AlgorithmCheckerList< T... >	makeAlgoCheckerList (const T &... args)
AlgorithmCheckerList< ParentChecker, CRBAChecker, ABAChecker >	makeDefaultCheckerList ()
	Default checker-list, used as the default argument in Model::check().
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	nonLinearEffects (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the bias terms \( b(q,\dot{q}) \) of the Lagrangian dynamics: More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , class ConstraintModel , class ConstraintModelAllocator , class ConstraintData , class ConstraintDataAllocator >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	pv (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 > &tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &contact_models, std::vector< ConstraintData, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
	The Popov-Vereshchagin algorithm. It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system. All the quantities are expressed in the LOCAL coordinate systems of the joint frames. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	reachableWorkspace (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, Eigen::MatrixXd &vertex, const ReachableSetParams &params=ReachableSetParams())
	Computes the reachable workspace on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	reachableWorkspaceHull (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, ReachableSetResults &res, const ReachableSetParams &params=ReachableSetParams())
	Computes the convex Hull reachable workspace on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	reachableWorkspaceWithCollisions (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, Eigen::MatrixXd &vertex, const ReachableSetParams &params=ReachableSetParams())
	Computes the reachable workspace with respect to a geometry model on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	reachableWorkspaceWithCollisionsHull (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const GeometryModel &geom_model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, ReachableSetResults &res, const ReachableSetParams &params=ReachableSetParams())
	Computes the convex Hull of the reachable workspace with respect to a geometry model on a fixed time horizon. Make sure that reachable workspace takes into account collisions with environment. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	rnea (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)
	The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType1 , typename TangentVectorType2 , typename ForceDerived >
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType &	rnea (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, const std::vector< ForceDerived > &fext)
	The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system, the desired joint accelerations and the external forces. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorPool , typename TangentVectorPool1 , typename TangentVectorPool2 , typename TangentVectorPool3 >
void	rneaInParallel (const size_t num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
	The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations. More...
template<typename Scalar >
coal::Transform3s	toCoalTransform3s (const SE3Tpl< Scalar > &m)
template<typename Scalar >
PINOCCHIO_DEPRECATED coal::Transform3s	toFclTransform3f (const SE3Tpl< Scalar > &m)
SE3	toPinocchioSE3 (const coal::Transform3s &tf)
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	transformJointIntoMimic (const ModelTpl< Scalar, Options, JointCollectionTpl > &input_model, const JointIndex &index_mimicked, const JointIndex &index_mimicking, const Scalar &scaling, const Scalar &offset, ModelTpl< Scalar, Options, JointCollectionTpl > &output_model)
	Transform of a joint of the model into a mimic joint. Keep the type of the joint as it was previously. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
const DataTpl< Scalar, Options, JointCollectionTpl >::SE3 &	updateFramePlacement (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
	Updates the placement of the given frame. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	updateFramePlacements (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Updates the position of each frame contained in the model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	updateGeometryPlacements (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data)
	Update the placement of the geometry objects according to the current joint placements contained in data. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
void	updateGeometryPlacements (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
	Apply a forward kinematics and update the placement of the geometry objects. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
void	updateGlobalPlacements (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
	Update the global placement of the joints oMi according to the relative placements of the joints. More...
API with return value as argument
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)
	Integrate a configuration vector for the specified model for a tangent vector during one unit time. More...
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 a configuration vector for the specified model for a tangent vector during one unit time. More...
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)
	Interpolate two configurations for a given model. More...
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 two configurations for a given model. More...
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)
	Compute the tangent vector that must be integrated during one unit time to go from q0 to q1. More...
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)
	Compute the tangent vector that must be integrated during one unit time to go from q0 to q1. More...
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)
	Squared distance between two configuration vectors. More...
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)
	Squared distance between two configuration vectors. More...
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)
	Generate a configuration vector uniformly sampled among provided limits. More...
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)
	Generate a configuration vector uniformly sampled among provided limits. More...
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)
	Return the neutral configuration element related to the model configuration space. More...
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)
	Return the neutral configuration element related to the model configuration space. More...
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)
	Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. More...
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)
	Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. More...
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)
	Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity. More...
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)
	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). More...
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)
	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). More...
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)
	Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV. More...
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)
	Set the tangentMap in a compact manner in a matrix of size nq x MAX_JOINT_NV. More...
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)
	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. More...
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)
	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. More...
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)
	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. More...
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)
	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. More...
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)
	Transport a matrix from the terminal to the initial tangent space of the integrate operation, with respect to the configuration or the velocity arguments. More...
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)
	Transport a matrix from the terminal to the initial tangent space of the integrate operation, with respect to the configuration or the velocity arguments. More...
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)
	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. More...
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)
	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. More...
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)
	Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity. More...
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)
	Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity. More...
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)
	Overall squared distance between two configuration vectors. More...
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)
	Overall squared distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2^{2} \). More...
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)
	Distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2 \). More...
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)
	Distance between two configuration vectors. More...
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)
	Normalize a configuration vector. More...
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 a configuration vector. More...
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())
	Check whether a configuration vector is normalized within the given precision provided by prec. More...
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())
	Check whether a configuration vector is normalized within the given precision provided by prec. More...
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())
	Return true if the given configurations are equivalents, within the given precision. More...
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 true if the given configurations are equivalents, within the given precision. More...
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)
	Return the Jacobian of the integrate function for the components of the config vector. More...
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)
	Return the Jacobian of the integrate function for the components of the config vector. More...
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)
	Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints. More...
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)
	Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints. More...
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)
	Return two vector where for each, the idx_v and v associated to the same atomic joint is given. More...

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

API that allocates memory
	Options
JointCollectionTpl &	model
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > &	lowerLimits
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > &	upperLimits
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>
ConfigVectorType	integrate (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Integrate a configuration vector for the specified model for a tangent vector during one unit time. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
ConfigVectorType	integrate (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
	Integrate a configuration vector for the specified model for a tangent vector during one unit time. More...
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>
ConfigVectorIn1	interpolate (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u)
	Interpolate two configurations for a given model. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
ConfigVectorIn1	interpolate (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u)
	Interpolate two configurations for a given model. More...
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>
ConfigVectorIn1	difference (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
	Compute the tangent vector that must be integrated during one unit time to go from q0 to q1. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
ConfigVectorIn1	difference (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
	Compute the tangent vector that must be integrated during one unit time to go from q0 to q1. More...
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>
ConfigVectorIn1	squaredDistance (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
	Squared distance between two configurations. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
ConfigVectorIn1	squaredDistance (const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
	Squared distance between two configuration vectors. More...
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>
	PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS ((typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)) randomConfiguration(const ModelTpl< Scalar
	Generate a configuration vector uniformly sampled among given limits. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
	PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS ((typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)) randomConfiguration(const ModelTpl< Scalar
	Generate a configuration vector uniformly sampled among provided limits. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
	PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS ((typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)) randomConfiguration(const ModelTpl< Scalar
	Generate a configuration vector uniformly sampled among the joint limits of the specified Model. More...
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)
	Return the neutral configuration element related to the model configuration space. More...
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 the neutral configuration element related to the model configuration space. More...
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
LieGroup_t::template operationProduct< Scalar, Options >::type	lieGroup (const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
	Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints. More...
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
LieGroupMap::template operationProduct< Scalar, Options >::type	lieGroup (const ModelTpl< Scalar, Options, JointCollectionTpl > &model)
	Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints. More...

Detailed Description

Main pinocchio namespace.

Enumeration Type Documentation

ADMMProximalRule

enum ADMMProximalRule : char

strong

ADMM proximal policy. MANUAL: mu_prox is constant and manually set. It is scaled by tau_prox AUTOMATIC: mu_prox is always set to rho.

Note

mu_prox is always scaled by tau_prox.

Definition at line 95 of file admm-solver.hpp.

Function Documentation

aba() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::aba	(	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 > &	tau,
		const Convention	convention = Convention::LOCAL
	)

The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	convention	Convention to use.

Note

When running in LOCAL convention data.f can be leaved in an inconsistent state.

Returns

The current joint acceleration stored in data.ddq.

aba() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::aba	(	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 > &	tau,
		const std::vector< SpatialForce, SpatialForceAllocator > &	fext,
		const Convention	convention = Convention::LOCAL
	)

The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given the current state and actuation of the model and the external forces.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
ForceDerived	Type of the external forces.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	fext	Vector of external forces expressed in the local frame of the joints (dim model.njoints)
[in]	convention	Convention to use.

Note

When running in LOCAL convention data.f can be leaved in an inconsistent state.

Returns

The current joint acceleration stored in data.ddq.

abaInParallel()

void pinocchio::abaInParallel	(	const size_t	num_threads,
		ModelPoolTpl< Scalar, Options, JointCollectionTpl > &	pool,
		const Eigen::MatrixBase< ConfigVectorPool > &	q,
		const Eigen::MatrixBase< TangentVectorPool1 > &	v,
		const Eigen::MatrixBase< TangentVectorPool2 > &	tau,
		const Eigen::MatrixBase< TangentVectorPool3 > &	a
	)

A parallel version of the Articulated Body algorithm. It computes the forward dynamics, aka the joint acceleration according to the current state of the system and the desired joint torque.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorPool	Matrix type of the joint configuration vector.
TangentVectorPool1	Matrix type of the joint velocity vector.
TangentVectorPool2	Matrix type of the joint torque vector.
TangentVectorPool3	Matrix type of the joint acceleration vector.

Parameters

[in]	pool	Pool containing model and data for parallel computations.
[in]	num_threads	Number of threads used for parallel computations.
[in]	q	The joint configuration vector (dim model.nq x batch_size).
[in]	v	The joint velocity vector (dim model.nv x batch_size).
[in]	tau	The joint acceleration vector (dim model.nv x batch_size).
[out]	a	The joint torque vector (dim model.nv x batch_size).

appendGeometryModel()

void pinocchio::appendGeometryModel ( GeometryModel &  geom_model1, const GeometryModel &  geom_model2  )

inline

Append geom_model2 to geom_model1

The steps for appending are:

• add GeometryObject of geom_model2 to geom_model1,
• add the collision pairs of geom_model2 into geom_model1 (indexes are updated)
• add all the collision pairs between geometry objects of geom_model1 and geom_model2. It is possible to ommit both data (an additional function signature is available which makes them optional), then inner/outer objects are not updated.

Parameters

[out]	geom_model1	geometry model where the data is added
[in]	geom_model2	geometry model from which new geometries are taken

Note

Of course, the geom_data corresponding to geom_model1 will not be valid anymore, and should be updated (or more simply, re-created from the new setting of geom_model1).

Todo:

This function is not asserted in unittest.

appendModel() [1/3]

ModelTpl<Scalar, Options, JointCollectionTpl> pinocchio::appendModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelA,
		const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelB,
		const FrameIndex	frameInModelA,
		const SE3Tpl< Scalar, Options > &	aMb
	)

Append a child model into a parent model, after a specific frame given by its index.

Parameters

[in]	modelA	the parent model.
[in]	modelB	the child model.
[in]	frameInModelA	index of the frame of modelA where to append modelB.
[in]	aMb	pose of modelB universe joint (index 0) in frameInModelA.

Returns

A new model containing the fusion of modelA and modelB.

The order of the joints in the output models are

• joints of modelA up to the parent of FrameInModelA,
• all the descendents of parent of FrameInModelA,
• the remaining joints of modelA.

Definition at line 74 of file model.hpp.

appendModel() [2/3]

void pinocchio::appendModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelA,
		const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelB,
		const FrameIndex	frameInModelA,
		const SE3Tpl< Scalar, Options > &	aMb,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	model
	)

Append a child model into a parent model, after a specific frame given by its index.

Parameters

[in]	modelA	the parent model.
[in]	modelB	the child model.
[in]	frameInModelA	index of the frame of modelA where to append modelB.
[in]	aMb	pose of modelB universe joint (index 0) in frameInModelA.
[out]	model	the resulting model.

The order of the joints in the output models are

• joints of modelA up to the parent of FrameInModelA,
• all the descendents of parent of FrameInModelA,
• the remaining joints of modelA.

appendModel() [3/3]

void pinocchio::appendModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelA,
		const ModelTpl< Scalar, Options, JointCollectionTpl > &	modelB,
		const GeometryModel &	geomModelA,
		const GeometryModel &	geomModelB,
		const FrameIndex	frameInModelA,
		const SE3Tpl< Scalar, Options > &	aMb,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		GeometryModel &	geomModel
	)

Append a child model into a parent model, after a specific frame given by its index.

Parameters

[in]	modelA	the parent model.
[in]	modelB	the child model.
[in]	geomModelA	the parent geometry model.
[in]	geomModelB	the child geometry model.
[in]	frameInModelA	index of the frame of modelA where to append modelB.
[in]	aMb	pose of modelB universe joint (index 0) in frameInModelA.
[out]	model	the resulting model.
[out]	geomModel	the resulting geometry model.

bodyRegressor() [1/2]

Eigen::Matrix< typename MotionVelocity::Scalar, 6, 10, typename MotionVelocity::Vector3 ::Options> pinocchio::bodyRegressor ( const MotionDense< MotionVelocity > &  v, const MotionDense< MotionAcceleration > &  a  )

inline

Computes the regressor for the dynamic parameters of a single rigid body.

The result is such that \( I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \)

Parameters

[in]	v	Velocity of the rigid body
[in]	a	Acceleration of the rigid body

Returns

The regressor of the body.

bodyRegressor() [2/2]

void pinocchio::bodyRegressor ( const MotionDense< MotionVelocity > &  v, const MotionDense< MotionAcceleration > &  a, const Eigen::MatrixBase< OutputType > &  regressor  )

inline

Computes the regressor for the dynamic parameters of a single rigid body.

The result is such that \( I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \)

Parameters

[in]	v	Velocity of the rigid body
[in]	a	Acceleration of the rigid body
[out]	regressor	The resulting regressor of the body.

buildMimicModel()

void pinocchio::buildMimicModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	input_model,
		const std::vector< JointIndex > &	index_mimicked,
		const std::vector< JointIndex > &	index_mimicking,
		const std::vector< Scalar > &	scaling,
		const std::vector< Scalar > &	offset,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	output_model
	)

Transform joints of a model into mimic joints.

Parameters

[in]	input_model	the input model to take joints from.
[in]	index_mimicked	indexes of the joint to mimic
[in]	index_mimicking	indexes of the joint that will mimic
[in]	scaling	Scalings of joint velocity and configuration
[in]	offset	Offsets of joint configuration
[out]	output_model	Model with the joint mimic

buildReducedModel() [1/4]

void pinocchio::buildReducedModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const GeometryModel &	geom_model,
		const std::vector< JointIndex > &	list_of_joints_to_lock,
		const Eigen::MatrixBase< ConfigVectorType > &	reference_configuration,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	reduced_model,
		GeometryModel &	reduced_geom_model
	)

Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock.

Parameters

[in]	model	the input model to reduce.
[in]	geom_model	the input geometry model to reduce.
[in]	list_of_joints_to_lock	list of joints to lock in the input model.
[in]	reference_configuration	reference configuration.
[out]	reduced_model	the reduced model.
[out]	reduced_geom_model	the reduced geometry model.

Remarks

All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.

buildReducedModel() [2/4]

void pinocchio::buildReducedModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const std::vector< GeometryModel > &	list_of_geom_models,
		const std::vector< JointIndex > &	list_of_joints_to_lock,
		const Eigen::MatrixBase< ConfigVectorType > &	reference_configuration,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	reduced_model,
		std::vector< GeometryModel > &	list_of_reduced_geom_models
	)

Build a reduced model and a reduced geometry model from a given input model, a given input geometry model and a list of joint to lock.

Parameters

[in]	model	the input model to reduce.
[in]	list_of_geom_models	the input geometry model to reduce (example: visual_model, collision_model).
[in]	list_of_joints_to_lock	list of joints to lock in the input model.
[in]	reference_configuration	reference configuration.
[out]	reduced_model	the reduced model.
[out]	list_of_reduced_geom_models	the list of reduced geometry models.

Remarks

All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.

buildReducedModel() [3/4]

ModelTpl<Scalar, Options, JointCollectionTpl> pinocchio::buildReducedModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const std::vector< JointIndex > &	list_of_joints_to_lock,
		const Eigen::MatrixBase< ConfigVectorType > &	reference_configuration
	)

Build a reduced model from a given input model and a list of joint to lock.

Parameters

[in]	model	the input model to reduce.
[in]	list_of_joints_to_lock	list of joints to lock in the input model.
[in]	reference_configuration	reference configuration.

Returns

A reduce model of the input model.

Remarks

All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.

Definition at line 157 of file model.hpp.

buildReducedModel() [4/4]

void pinocchio::buildReducedModel	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		std::vector< JointIndex >	list_of_joints_to_lock,
		const Eigen::MatrixBase< ConfigVectorType > &	reference_configuration,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	reduced_model
	)

Build a reduced model from a given input model and a list of joint to lock.

Parameters

[in]	model	the input model to reduce.
[in]	list_of_joints_to_lock	list of joints to lock in the input model.
[in]	reference_configuration	reference configuration.
[out]	reduced_model	the reduced model.

Remarks

All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.

Todo:

At the moment, the joint and geometry order is kept while the frames are re-ordered in a hard to predict way. Their order could be kept.

ccrba()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::ccrba	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta according to the current joint configuration and velocity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The Centroidal Momentum Matrix Ag.

Remarks

As another output, this algorithm also computes the Joint Jacobian matrix (accessible via data.J).

centerOfMass() [1/5]

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::centerOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const bool	computeSubtreeComs = true
	)

Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data. The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees, expressed in the local coordinate frame of each joint.

Definition at line 216 of file center-of-mass.hpp.

centerOfMass() [2/5]

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::centerOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const bool	computeSubtreeComs = true
	)

Computes the center of mass position of a given model according to a particular joint configuration. The result is accessible through data.com[0] for the full body com and data.com[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees.

Returns

The center of mass position of the full rigid body system expressed in the world frame.

centerOfMass() [3/5]

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::centerOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v,
		const bool	computeSubtreeComs = true
	)

Computes the center of mass position and velocity of a given model according to a particular joint configuration and velocity. The result is accessible through data.com[0], data.vcom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees.

Returns

The center of mass position of the full rigid body system expressed in the world frame.

centerOfMass() [4/5]

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::centerOfMass	(	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,
		const bool	computeSubtreeComs = true
	)

Computes the center of mass position, velocity and acceleration of a given model according to a particular joint configuration, velocity and acceleration. The result is accessible through data.com[0], data.vcom[0], data.acom[0] for the full body com position, velocity and acceleation. And data.com[i], data.vcom[i] and data.acom[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[in]	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees.

Returns

The center of mass position of the full rigid body system expressed in the world frame.

centerOfMass() [5/5]

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::centerOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		KinematicLevel	kinematic_level,
		const bool	computeSubtreeComs = true
	)

Computes the center of mass position, velocity and acceleration of a given model according to the current kinematic values contained in data and the requested kinematic_level. The result is accessible through data.com[0], data.vcom[0] and data.acom[0] for the full body com position and velocity. And data.com[i] and data.vcom[i] for the subtree supported by joint i (expressed in the joint i frame).

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	kinematic_level	if = POSITION, computes the CoM position, if = VELOCITY, also computes the CoM velocity and if = ACCELERATION, it also computes the CoM acceleration.
[in]	computeSubtreeComs	If true, the algorithm computes also the center of mass of the subtrees.

checkData()

bool pinocchio::checkData	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Check the validity of data wrt to model, in particular if model has been modified.

Parameters

[in]	model	reference model
[in]	data	corresponding data

Returns

True if data is valid wrt model.

compactTangentMap() [1/2]

void pinocchio::compactTangentMap	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const std::vector< JointIndex > &	joint_ids,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentMapMatrixType > &	TMc
	)

Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	joint_ids	Joint to condider to compute the tangentMap
[in]	q	Initial configuration (size model.nq)
[out]	TMc	Compact storage of the tangent map space.

Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	joint_ids	Joint to condider to compute the tangentMap
[in]	q	Initial configuration (size model.nq)
[out]	TMc	Compact storage of the tangent map space.

Definition at line 654 of file joint-configuration.hpp.

compactTangentMap() [2/2]

void pinocchio::compactTangentMap	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const std::vector< JointIndex > &	joint_ids,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentMapMatrixType > &	TMc
	)

Set the tangentMap in a compact manner in a matrix of size nq x MAX_JOINT_NV.

Set the tangentMap in a compact manner in matric of size nq x MAX_JOINT_NV.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	joint_ids	Joint to condider to compute the tangentMap
[in]	q	Initial configuration (size model.nq)
[out]	TMc	Compact storage of the tangent map space.

Definition at line 654 of file joint-configuration.hpp.

computeABADerivatives() [1/8]

void pinocchio::computeABADerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

The derivatives of the Articulated-Body algorithm. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

See also

pinocchio::aba

computeABADerivatives() [2/8]

std::enable_if_t< ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime || TangentVectorType2::IsVectorAtCompileTime, void> pinocchio::computeABADerivatives	(	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 > &	tau
	)

The derivatives of the Articulated-Body algorithm.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).

Returns

The results are stored in data.ddq_dq, data.ddq_dv and data.Minv which respectively correspond to the partial derivatives of the joint acceleration vector with respect to the joint configuration, velocity and torque. And as for pinocchio::computeMinverse, only the upper triangular part of data.Minv is filled.

See also

pinocchio::aba and

pinocchio::computeABADerivatives.

computeABADerivatives() [3/8]

void pinocchio::computeABADerivatives	(	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 > &	tau,
		const Eigen::MatrixBase< MatrixType1 > &	aba_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	aba_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	aba_partial_dtau
	)

The derivatives of the Articulated-Body algorithm.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[out]	aba_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	aba_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	aba_partial_dtau	Partial derivative of the generalized torque vector with respect to the joint torque.

Note

aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix.

See also

pinocchio::aba

computeABADerivatives() [4/8]

void pinocchio::computeABADerivatives	(	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 > &	tau,
		const std::vector< SpatialForce, SpatialForceAllocator > &	fext
	)

The derivatives of the Articulated-Body algorithm with external forces.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).

Note

The results are stored in data.ddq_dq, data.ddq_dv and data.Minv which respectively correspond to the partial derivatives of the joint acceleration vector with respect to the joint configuration, velocity and torque. And as for pinocchio::computeMinverse, only the upper triangular part of data.Minv is filled.

See also

pinocchio::aba and

pinocchio::computeABADerivatives.

computeABADerivatives() [5/8]

void pinocchio::computeABADerivatives	(	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 > &	tau,
		const std::vector< SpatialForce, SpatialForceAllocator > &	fext,
		const Eigen::MatrixBase< MatrixType1 > &	aba_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	aba_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	aba_partial_dtau
	)

The derivatives of the Articulated-Body algorithm with external forces.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).
[out]	aba_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	aba_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	aba_partial_dtau	Partial derivative of the generalized torque vector with respect to the joint torque.

Note

aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix.

See also

pinocchio::aba

computeABADerivatives() [6/8]

std::enable_if_t< !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime || MatrixType3::IsVectorAtCompileTime), void> pinocchio::computeABADerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< MatrixType1 > &	aba_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	aba_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	aba_partial_dtau
	)

The derivatives of the Articulated-Body algorithm. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden.

Template Parameters

JointCollection	Collection of Joint types.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[out]	aba_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	aba_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	aba_partial_dtau	Partial derivative of the generalized torque vector with respect to the joint torque.

Note

aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix.

See also

pinocchio::aba

computeABADerivatives() [7/8]

void pinocchio::computeABADerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const std::vector< SpatialForce, SpatialForceAllocator > &	fext
	)

The derivatives of the Articulated-Body algorithm with external forces. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).

Note

aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix.

See also

pinocchio::aba

computeABADerivatives() [8/8]

void pinocchio::computeABADerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const std::vector< SpatialForce, SpatialForceAllocator > &	fext,
		const Eigen::MatrixBase< MatrixType1 > &	aba_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	aba_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	aba_partial_dtau
	)

The derivatives of the Articulated-Body algorithm with external forces. This function exploits the internal computations made in pinocchio::aba to significantly reduced the computation burden.

Template Parameters

JointCollection	Collection of Joint types.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint torque vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).
[out]	aba_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	aba_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	aba_partial_dtau	Partial derivative of the generalized torque vector with respect to the joint torque.

Note

aba_partial_dtau is in fact nothing more than the inverse of the joint space inertia matrix.

See also

pinocchio::aba

computeAllTerms()

void pinocchio::computeAllTerms	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the same time to:

• pinocchio::forwardKinematics
• pinocchio::crba
• pinocchio::nonLinearEffects
• pinocchio::computeJointJacobians
• pinocchio::centerOfMass
• pinocchio::jacobianCenterOfMass
• pinocchio::ccrba
• pinocchio::computeKineticEnergy
• pinocchio::computePotentialEnergy
• pinocchio::computeGeneralizedGravity

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Note

All the results are stored in data. Please refer to the specific algorithm for further details.

computeBodyRadius()

void pinocchio::computeBodyRadius	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const GeometryModel &	geom_model,
		GeometryData &	geom_data
	)

Compute the radius of the geometry volumes attached to every joints.

Parameters

[in]	model	Kinematic model of the system
[in]	geom_model	Geometry model of the system
[out]	geom_data	Geometry data of the system

See also

GeometryData::radius

computeCentroidalDynamicsDerivatives()

void pinocchio::computeCentroidalDynamicsDerivatives	(	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,
		const Eigen::MatrixBase< Matrix6xLike0 > &	dh_dq,
		const Eigen::MatrixBase< Matrix6xLike1 > &	dhdot_dq,
		const Eigen::MatrixBase< Matrix6xLike2 > &	dhdot_dv,
		const Eigen::MatrixBase< Matrix6xLike3 > &	dhdot_da
	)

Computes the analytical derivatives of the centroidal dynamics with respect to the joint configuration vector, velocity and acceleration.

Computes the first order approximation of the centroidal dynamics time derivative and corresponds to the following equation \( d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q} + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v} + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a} \)

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[out]	dh_dq	The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv).
[out]	dhdot_dq	The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).
[out]	dhdot_dv	The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).
[out]	dhdot_da	The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).

Note

It also computes the current centroidal dynamics and its time derivative. For information, the centroidal momentum matrix is equivalent to dhdot_da.

computeCentroidalMap()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::computeCentroidalMap	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes the Centroidal Momentum Matrix.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The Centroidal Momentum Matrix Ag.

Remarks

As another output, this algorithm also computes the Joint Jacobian matrix (accessible via data.J).

computeCentroidalMapTimeVariation()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::computeCentroidalMapTimeVariation	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the Centroidal Momentum Matrix time derivative.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The Centroidal Momentum Matrix time derivative dAg (accessible via data.dAg).

Remarks

As another output, this algorithm also computes the Centroidal Momentum Matrix Ag (accessible via data.Ag), the Joint Jacobian matrix (accessible via data.J) and the time derivatibe of the Joint Jacobian matrix (accessible via data.dJ).

computeCentroidalMomentum() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Force& pinocchio::computeCentroidalMomentum	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center of mass.

Template Parameters

Scalar	The scalar type.
Options	Eigen Alignment options.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])

computeCentroidalMomentum() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Force& pinocchio::computeCentroidalMomentum	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center of mass.

Template Parameters

Scalar	The scalar type.
Options	Eigen Alignment options.
JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])

computeCentroidalMomentumTimeVariation() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Force& pinocchio::computeCentroidalMomentumTimeVariation	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative expressed around the center of mass.

Template Parameters

Scalar	The scalar type.
Options	Eigen Alignment options.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])

computeCentroidalMomentumTimeVariation() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Force& pinocchio::computeCentroidalMomentumTimeVariation	(	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
	)

Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative expressed around the center of mass.

Template Parameters

Scalar	The scalar type.
Options	Eigen Alignment options.
JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).

Returns

The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])

computeCollision() [1/2]

bool pinocchio::computeCollision	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const PairIndex	pair_id
	)

Compute the collision status between a SINGLE collision pair. The result is store in the collisionResults vector.

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.
[in]	pair_id	The collsion pair index in the GeometryModel.

Returns

Return true is the collision objects are colliding.

Note

The complete collision result is also available in geom_data.collisionResults[pair_id]

computeCollision() [2/2]

bool pinocchio::computeCollision	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const PairIndex	pair_id,
		coal::CollisionRequest &	collision_request
	)

Compute the collision status between a SINGLE collision pair. The result is store in the collisionResults vector.

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.
[in]	pair_id	The collsion pair index in the GeometryModel.
[in]	collision_request	The collision request associated to the collision pair.

Returns

Return true is the collision objects are colliding.

Note

The complete collision result is also available in geom_data.collisionResults[pair_id]

computeCollisions() [1/6]

bool pinocchio::computeCollisions	(	BroadPhaseManagerBase< BroadPhaseManagerDerived > &	broadphase_manager,
		CollisionCallBackBase *	callback
	)

Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements and broadphase_manager.update() have been called first.

Parameters

[in]	broadphase_manager	broadphase instance for collision detection.
[in]	callback	callback pointer used for collision detection.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

computeCollisions() [2/6]

bool pinocchio::computeCollisions	(	BroadPhaseManagerBase< BroadPhaseManagerDerived > &	broadphase_manager,
		const bool	stopAtFirstCollision = false
	)

Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements and broadphase_manager.update() have been called first.

Parameters

[in]	broadphase_manager	broadphase instance for collision detection.
[in]	stopAtFirstCollision	if true, stop the loop over the collision pairs when the first collision is detected.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

computeCollisions() [3/6]

bool pinocchio::computeCollisions	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const bool	stopAtFirstCollision = false
	)

Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeometryPlacements has been called first.

Parameters

[in]	geom_model	geometry model (const)
[out]	geom_data	corresponding geometry data (nonconst) where collisions are computed
[in]	stopAtFirstCollision	if true, stop the loop over the collision pairs when the first collision is detected.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

computeCollisions() [4/6]

bool pinocchio::computeCollisions ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, BroadPhaseManagerBase< BroadPhaseManagerDerived > &  broadphase_manager, CollisionCallBackBase *  callback, const Eigen::MatrixBase< ConfigVectorType > &  q  )

inline

Compute the forward kinematics, update the geometry placements and run the collision detection using the broadphase manager.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	robot model (const)
[out]	data	corresponding data (nonconst) where the forward kinematics results are stored
[in]	broadphase_manager	broadphase manager for collision detection.
[in]	callback	callback pointer used for collision detection.///
[in]	q	robot configuration.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

Note

A similar function is available without model, data and q, not recomputing the forward kinematics.

computeCollisions() [5/6]

bool pinocchio::computeCollisions ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, BroadPhaseManagerBase< BroadPhaseManagerDerived > &  broadphase_manager, const Eigen::MatrixBase< ConfigVectorType > &  q, const bool  stopAtFirstCollision = false  )

inline

Compute the forward kinematics, update the geometry placements and run the collision detection using the broadphase manager.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	robot model (const)
[out]	data	corresponding data (nonconst) where the forward kinematics results are stored
[in]	broadphase_manager	broadphase manager for collision detection.
[in]	q	robot configuration.
[in]	stopAtFirstCollision	if true, stop the loop over the collision pairs when the first collision is detected.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

Note

A similar function is available without model, data and q, not recomputing the forward kinematics.

computeCollisions() [6/6]

bool pinocchio::computeCollisions	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const bool	stopAtFirstCollision = false
	)

Compute the forward kinematics, update the geometry placements and calls computeCollision for every active pairs of GeometryData.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	robot model (const)
[out]	data	corresponding data (nonconst) where the forward kinematics results are stored
[in]	geom_model	geometry model (const)
[out]	geom_data	corresponding geometry data (nonconst) where distances are computed
[in]	q	robot configuration.
[in]	stopAtFirstCollision	if true, stop the loop over the collision pairs when the first collision is detected.

Warning

if stopAtFirstcollision = true, then the collisions vector will not be entirely fulfilled (of course).

Note

A similar function is available without model, data and q, not recomputing the forward kinematics.

computeContactPatch()

void pinocchio::computeContactPatch	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const PairIndex	pair_id
	)

Compute the contact patch info associated with the collision pair given by pair_id. Note that an actual computation will only occur if the collision pair is indeed in collision (i.e. only if geom_data.collisionResult[pair_id].isCollision() is true).

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.
[in]	pair_id	The collsion pair index in the GeometryModel.

Note

The complete contact patch result is available in geom_data.contactPatchResults[pair_id]

computeContactPatches()

void pinocchio::computeContactPatches	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data
	)

Calls computeContactPatch for every collision pair.

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.

Note

This function must be called after computeCollisions.

computeCoriolisMatrix()

const DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs& pinocchio::computeCoriolisMatrix	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the Coriolis Matrix \( C(q,\dot{q}) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \end{eqnarray} \)

Note

In the previous equation, \( c(q, \dot{q}) = C(q, \dot{q})\dot{q} \).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The Coriolis matrix stored in data.C.

computeDampedDelassusMatrixInverse()

void pinocchio::computeDampedDelassusMatrixInverse	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &	contact_models,
		std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &	contact_data,
		const Eigen::MatrixBase< MatrixType > &	damped_delassus_inverse,
		const Scalar	mu,
		const bool	scaled = false,
		const bool	Pv = true
	)

Computes the inverse of the Delassus matrix associated to a set of given constraints.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
ModelAllocator	Allocator class for the std::vector.
DataAllocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (size model.nq).
[in]	contact_models	Vector of contact models.
[in]	contact_data	Vector of contact data.
[out]	damped_delassus_inverse	The resulting damped Delassus matrix.
[in]	mu	Damping factor well-posdnessed of the problem.
[in]	scaled	If set to true, the solution is scaled my a factor \( \mu \) to avoid numerical rounding issues.
[in]	Pv	If set to true, uses PV-OSIMr, otherwise uses EFPA.

Note

A hint: a typical value for mu is 1e-4 when two contact constraints or more are redundant.

computeDelassusMatrix()

void pinocchio::computeDelassusMatrix	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &	contact_models,
		std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &	contact_data,
		const Eigen::MatrixBase< MatrixType > &	delassus,
		const Scalar	mu = 0
	)

Computes the Delassus matrix associated to a set of given constraints.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
ModelAllocator	Allocator class for the std::vector.
DataAllocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (size model.nq).
[in]	contact_models	Vector of contact models.
[in]	contact_data	Vector of contact data.
[out]	delassus	The resulting Delassus matrix.
[in]	mu	Optional damping factor used when computing the inverse of the Delassus matrix.

computeDistance()

coal::DistanceResult& pinocchio::computeDistance	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const PairIndex	pair_id
	)

Compute the minimal distance between collision objects of a SINGLE collison pair.

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.
[in]	pair_id	The index of the collision pair in geom model.

Returns

A reference on coal struct containing the distance result, referring an element of vector geom_data::distanceResults.

Note

The complete distance result is also available in geom_data.distanceResults[pair_id]

computeDistances() [1/3]

std::size_t pinocchio::computeDistances	(	const GeometryModel &	geom_model,
		GeometryData &	geom_data
	)

Compute the minimal distance between collision objects of a ALL collison pair.

Parameters

[in]	geom_model	the geometry model (const)
[out]	geom_data	the corresponding geometry data, where computations are done.

Returns

Index of the minimal pair distance in geom_data.DistanceResult

Note

The complete distance result is available by pair in geom_data.distanceResults

computeDistances() [2/3]

std::size_t pinocchio::computeDistances	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const GeometryModel &	geom_model,
		GeometryData &	geom_data
	)

Update the geometry placements and calls computeDistance for every active pairs of GeometryData.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	robot model (const)
[in]	data	corresponding data (nonconst) where FK results are stored
[in]	geom_model	geometry model (const)
[out]	geom_data	corresponding geometry data (nonconst) where distances are computed

Note

A similar function is available without model, data and q, not recomputing the FK.

computeDistances() [3/3]

std::size_t pinocchio::computeDistances	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const GeometryModel &	geom_model,
		GeometryData &	geom_data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Compute the forward kinematics, update the geometry placements and calls computeDistance for every active pairs of GeometryData.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	robot model (const)
[in]	data	corresponding data (nonconst) where FK results are stored
[in]	geom_model	geometry model (const)
[out]	geom_data	corresponding geometry data (nonconst) where distances are computed
[in]	q	robot configuration.

Note

A similar function is available without model, data and q, not recomputing the FK.

computeForwardKinematicsDerivatives() [1/2]

void pinocchio::computeForwardKinematicsDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes all the terms required to compute the derivatives of the placement, spatial velocity for any joint of the model.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[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).

computeForwardKinematicsDerivatives() [2/2]

void pinocchio::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
	)

Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration for any joint of the model.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	v	The joint velocity (vector dim model.nv).
[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).

computeFrameJacobian() [1/2]

void pinocchio::computeFrameJacobian ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< ConfigVectorType > &  q, const FrameIndex  frame_id, const Eigen::MatrixBase< Matrix6xLike > &  J  )

inline

Computes the Jacobian of a specific Frame expressed in the LOCAL frame coordinate system.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	frame_id	The id of the Frame refering to model.frames[frame_id].
[out]	J	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().

Remarks

The result of this function is equivalent to call first computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call getJointJacobian(model,data,jointId,LOCAL,J), but forwardKinematics and updateFramePlacements are not fully computed.

Definition at line 491 of file frames.hpp.

computeFrameJacobian() [2/2]

void pinocchio::computeFrameJacobian ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< ConfigVectorType > &  q, const FrameIndex  frame_id, const ReferenceFrame  reference_frame, const Eigen::MatrixBase< Matrix6xLike > &  J  )

inline

Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	frame_id	The id of the Frame refering to model.frames[frame_id].
[in]	reference_frame	Reference frame in which the Jacobian is expressed.
[out]	J	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().

Remarks

The result of this function is equivalent to call first computeJointJacobians(model,data,q), then updateFramePlacements(model,data) and then call getJointJacobian(model,data,jointId,rf,J), but forwardKinematics and updateFramePlacements are not fully computed.

computeFrameKinematicRegressor() [1/2]

DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x pinocchio::computeFrameKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		const ReferenceFrame	rf
	)

Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the frame given as input.

Remarks

It assumes that the framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	Index of the frame.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).

Definition at line 169 of file regressor.hpp.

computeFrameKinematicRegressor() [2/2]

void pinocchio::computeFrameKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		const ReferenceFrame	rf,
		const Eigen::MatrixBase< Matrix6xReturnType > &	kinematic_regressor
	)

Remarks

It assumes that the framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	Index of the frame.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
[out]	kinematic_regressor	The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.

computeGeneralizedGravity()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::computeGeneralizedGravity	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes the generalized gravity contribution \( g(q) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + c(q, \dot{q}) + g(q) = \tau \end{eqnarray} \)

Note

This function is equivalent to pinocchio::rnea(model, data, q, 0, 0).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The generalized gravity torque stored in data.g.

computeGeneralizedGravityDerivatives()

void pinocchio::computeGeneralizedGravityDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< ReturnMatrixType > &	gravity_partial_dq
	)

Computes the partial derivative of the generalized gravity contribution with respect to the joint configuration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
ReturnMatrixType	Type of the matrix containing the partial derivative of the gravity vector with respect to the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[out]	gravity_partial_dq	Partial derivative of the generalized gravity vector with respect to the joint configuration.

Remarks

gravity_partial_dq must be first initialized with zeros (gravity_partial_dq.setZero).

See also

pinocchio::computeGeneralizedGravity

computeInverseDynamicsConstraintForces()

bool pinocchio::computeInverseDynamicsConstraintForces	(	const PointContactConstraintModelVector &	constraint_models,
		const PointContactConstraintDataVector &	constraint_datas,
		const Eigen::MatrixBase< VectorLikeC > &	c_ref,
		const Eigen::MatrixBase< VectorLikeResult > &	_lambda,
		ProximalSettingsTpl< Scalar > &	settings,
		bool	solve_ncp = true
	)

Compute the contact forces given a target velocity of contact points.

Parameters

[in]	constraint_models	The vector of constraint models.
[in]	constraint_datas	Vector of constraint datas.
[in]	c_ref	The desired constraint velocity.
[in,out]	_lambda	Vector of solution. Should be initialized with zeros or from an initial estimate.
[in,out]	settings	The settings for the proximal algorithm
[in]	solve_ncp	whether to solve the NCP (true) or CCP (false).

computeJointJacobian()

void pinocchio::computeJointJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const JointIndex	joint_id,
		const Eigen::MatrixBase< Matrix6Like > &	J
	)

Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store the result in the input argument J.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	joint_id	The id of the joint refering to model.joints[jointId].
[out]	J	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().

Remarks

The result of this function is equivalent to call first computeJointJacobians(model,data,q) and then call getJointJacobian(model,data,jointId,LOCAL,J), but forwardKinematics is not fully computed. It is worth to call jacobian if you only need a single Jacobian for a specific joint. Otherwise, for several Jacobians, it is better to call computeJointJacobians(model,data,q) followed by getJointJacobian(model,data,jointId,LOCAL,J) for each Jacobian.

computeJointJacobians() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::computeJointJacobians	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame. The result is accessible through data.J. This function assumes that pinocchio::forwardKinematics has been called before.

Note

This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame.

See also

pinocchio::getJointJacobian for doing this specific extraction.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The full model Jacobian (matrix 6 x model.nv).

computeJointJacobians() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::computeJointJacobians	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame. The result is accessible through data.J. This function computes also the forwardKinematics of the model.

Note

This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame.

See also

pinocchio::getJointJacobian for doing this specific extraction.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The full model Jacobian (matrix 6 x model.nv).

computeJointJacobiansTimeVariation()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::computeJointJacobiansTimeVariation	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v. The result is accessible through data.dJ.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The full model Jacobian (matrix 6 x model.nv).

computeJointKinematicHessians() [1/2]

void pinocchio::computeJointKinematicHessians	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

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 computeJointJacobians for such a function.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Remarks

This function is also related to

See also

getJointKinematicHessian.

computeJointKinematicHessians() [2/2]

void pinocchio::computeJointKinematicHessians	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes all the terms required to compute the second order derivatives of the placement information, also know as the kinematic Hessian.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).

Remarks

This function is also related to

See also

getJointKinematicHessian.

Definition at line 421 of file kinematics-derivatives.hpp.

computeJointKinematicRegressor() [1/4]

DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x pinocchio::computeJointKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	rf
	)

Computes the kinematic regressor that links the joint placement variations of the whole kinematic tree to the placement variation of the joint given as input.

Remarks

It assumes that the forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).

Definition at line 120 of file regressor.hpp.

computeJointKinematicRegressor() [2/4]

void pinocchio::computeJointKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	rf,
		const Eigen::MatrixBase< Matrix6xReturnType > &	kinematic_regressor
	)

Remarks

It assumes that the forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
[out]	kinematic_regressor	The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.

computeJointKinematicRegressor() [3/4]

DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x pinocchio::computeJointKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	rf,
		const SE3Tpl< Scalar, Options > &	placement
	)

Computes the kinematic regressor that links the joint placements variations of the whole kinematic tree to the placement variation of the frame rigidly attached to the joint and given by its placement w.r.t. to the joint frame.

Remarks

It assumes that the forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
[in]	placement	Relative placement to the joint frame.

Definition at line 70 of file regressor.hpp.

computeJointKinematicRegressor() [4/4]

void pinocchio::computeJointKinematicRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	rf,
		const SE3Tpl< Scalar, Options > &	placement,
		const Eigen::MatrixBase< Matrix6xReturnType > &	kinematic_regressor
	)

Remarks

It assumes that the forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> &, DataTpl<Scalar,Options,JointCollectionTpl> &, const Eigen::MatrixBase<ConfigVectorType> &) has been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint.
[in]	rf	Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
[in]	placement	Relative placement to the joint frame.
[out]	kinematic_regressor	The kinematic regressor containing the result. Matrix of size 6*(model.njoints-1) initialized to 0.

computeJointTorqueRegressor()

DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs& pinocchio::computeJointTorqueRegressor ( 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  )

inline

Computes the joint torque regressor that links the joint torque to the dynamic parameters of each link according to the current the robot motion.

The result is stored in data.jointTorqueRegressor and it corresponds to a matrix \( Y \) such that \( \tau = Y(q,\dot{q},\ddot{q})\pi \) where \( \pi = (\pi_1^T\ \dots\ \pi_n^T)^T \) and \( \pi_i = \text{model.inertias[i].toDynamicParameters()} \)

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).

Returns

The joint torque regressor of the system.

Warning

This function writes temporary information in data.bodyRegressor. This means if you have valuable data in it it will be overwritten.

computeKineticEnergy() [1/2]

Scalar pinocchio::computeKineticEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The kinetic energy of the system in [J].

computeKineticEnergy() [2/2]

Scalar pinocchio::computeKineticEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The kinetic energy of the system in [J].

computeKKTContactDynamicMatrixInverse()

void pinocchio::computeKKTContactDynamicMatrixInverse	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< ConstraintMatrixType > &	J,
		const Eigen::MatrixBase< KKTMatrixType > &	KKTMatrix_inv,
		const Scalar &	inv_damping = 0.
	)

Computes the inverse of the KKT matrix for dynamics with contact constraints. It computes the following matrix:

\( \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1} \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \)

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	J	Jacobian of the constraints.
[out]	KKTMatrix_inv	inverse of the MJtJ matrix.
[in]	inv_damping	regularization coefficient.

computeMechanicalEnergy() [1/2]

Scalar pinocchio::computeMechanicalEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the mechanical energy of the system stored in data.mechanical_energy. The result is accessible through data.kinetic_energy.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The total mechanal energy of the system in [J].

computeMechanicalEnergy() [2/2]

Scalar pinocchio::computeMechanicalEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the mechanical energy of the system stored in data.mechanical_energy. The result is accessible through data.kinetic_energy.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The total mechanal energy of the system in [J]. The fonctions also computes the data.kinetic_energy and data.potential_energy.

computeMinverse() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs& pinocchio::computeMinverse	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the inverse of the joint space inertia matrix using Articulated Body formulation. Compared to the complete signature computeMinverse<Scalar,Options,ConfigVectorType>, this version assumes that ABA has been called first.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The inverse of the joint space inertia matrix stored in data.ddq.

computeMinverse() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs& pinocchio::computeMinverse	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes the inverse of the joint space inertia matrix using Articulated Body formulation.

Remarks

Only the upper triangular part of the matrix is filled.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The inverse of the joint space inertia matrix stored in data.Minv.

computePotentialEnergy() [1/2]

Scalar pinocchio::computePotentialEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Computes the potential energy of the system, i.e. the potential energy linked to the gravity field. The result is accessible through data.potential_energy.

Template Parameters

JointCollection

Collection of Joint types.

Note

This potential energy are of the for \( \sum_{i} - m_{i}gh_{i} \) where:

• \( m_{i} \) is the mass of the body \( i \),
• \( h_{i} \) is the height of the body \( i \),
• \( g \) is the gravity value.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The potential energy of the system expressed in [J].

computePotentialEnergy() [2/2]

Scalar pinocchio::computePotentialEnergy	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Computes the potential energy of the system, i.e. the potential energy linked to the gravity field. The result is accessible through data.potential_energy.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Note

This potential energy are of the for \( \sum_{i} - m_{i}gh_{i} \) where:

• \( m_{i} \) is the mass of the body \( i \),
• \( h_{i} \) is the height of the body \( i \),
• \( g \) is the gravity value.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The potential energy of the system expressed in [J].

computePotentialEnergyRegressor()

const DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs& pinocchio::computePotentialEnergyRegressor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

\brief Computes the kinetic energy regressor that links the kinetic energy of the system to
the dynamic parameters of each link according to the current robot motion.

The result is stored in `data.kineticEnergyRegressor` and it corresponds to a matrix
\f$ Y_e \f$ such that \f$ K = Y_e(q,\dot{q})\pi \f$ where
\pi_i = \text{model.inertias[i].toDynamicParameters()} \f$

\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).

\return The kinetic energy regressor of the system.

template< typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs & computeKineticEnergyRegressor( const ModelTpl<Scalar, Options, JointCollectionTpl> & model, DataTpl<Scalar, Options, JointCollectionTpl> & data, const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<TangentVectorType> & v);

\brief Computes the potential energy regressor that links the potential energy
of the system to the dynamic parameters of each link according to the current robot motion.

The result is stored in `data.potentialEnergyRegressor` and it corresponds to a matrix
\f$ Y_p \f$ such that \f$ P = Y_p(q)\pi \f$ where
\pi_i = \text{model.inertias[i].toDynamicParameters()} \f$

\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).

\return The kinetic energy regressor of the system. 

computeRNEADerivatives() [1/4]

void pinocchio::computeRNEADerivatives	(	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
	)

Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).

Note

The results are stored in data.dtau_dq, data.dtau_dv and data.M which respectively correspond to the partial derivatives of the joint torque vector with respect to the joint configuration, velocity and acceleration. As for pinocchio::crba, only the upper triangular part of data.M is filled.

See also

pinocchio::rnea, pinocchio::crba, pinocchio::decompose

computeRNEADerivatives() [2/4]

void pinocchio::computeRNEADerivatives	(	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,
		const Eigen::MatrixBase< MatrixType1 > &	rnea_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	rnea_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	rnea_partial_da
	)

Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[out]	rnea_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	rnea_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	rnea_partial_da	Partial derivative of the generalized torque vector with respect to the joint acceleration.

Remarks

rnea_partial_dq, rnea_partial_dv and rnea_partial_da must be first initialized with zeros (rnea_partial_dq.setZero(),etc). As for pinocchio::crba, only the upper triangular part of rnea_partial_da is filled.

See also

pinocchio::rnea

computeRNEADerivatives() [3/4]

void pinocchio::computeRNEADerivatives	(	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,
		const std::vector< ForceTpl< Scalar, Options >> &	fext
	)

Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).

Note

The results are stored in data.dtau_dq, data.dtau_dv and data.M which respectively correspond to the partial derivatives of the joint torque vector with respect to the joint configuration, velocity and acceleration. As for pinocchio::crba, only the upper triangular part of data.M is filled.

See also

pinocchio::rnea, pinocchio::crba, pinocchio::decompose

computeRNEADerivatives() [4/4]

void pinocchio::computeRNEADerivatives	(	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,
		const std::vector< ForceTpl< Scalar, Options >> &	fext,
		const Eigen::MatrixBase< MatrixType1 > &	rnea_partial_dq,
		const Eigen::MatrixBase< MatrixType2 > &	rnea_partial_dv,
		const Eigen::MatrixBase< MatrixType3 > &	rnea_partial_da
	)

Computes the derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.
MatrixType1	Type of the matrix containing the partial derivative with respect to the joint configuration vector.
MatrixType2	Type of the matrix containing the partial derivative with respect to the joint velocity vector.
MatrixType3	Type of the matrix containing the partial derivative with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).
[out]	rnea_partial_dq	Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	rnea_partial_dv	Partial derivative of the generalized torque vector with respect to the joint velocity.
[out]	rnea_partial_da	Partial derivative of the generalized torque vector with respect to the joint acceleration.

Remarks

rnea_partial_dq, rnea_partial_dv and rnea_partial_da must be first initialized with zeros (rnea_partial_dq.setZero(),etc). As for pinocchio::crba, only the upper triangular part of rnea_partial_da is filled.

See also

pinocchio::rnea

ComputeRNEASecondOrderDerivatives() [1/2]

void pinocchio::ComputeRNEASecondOrderDerivatives ( 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  )

inline

Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).

Note

The results are stored in data.d2tau_dqdq, data.d2tau_dvdv, data.d2tau_dqdv, and data.d2tau_dadq which respectively correspond to the Second-Order partial derivatives of the joint torque vector with respect to the joint configuration, velocity and cross Second-Order partial derivatives with respect to configuration/velocity and configuration/acceleration respectively.

Remarks

d2tau_dqdq, d2tau_dvdv2, d2tau_dqdv and d2tau_dadq must be first initialized with zeros (d2tau_dqdq.setZero(),etc). The storage order of the 3D-tensor derivatives is important. For d2tau_dqdq, the elements of generalized torque varies along the rows, while elements of q vary along the columns and pages of the tensor. For d2tau_dqdv, the elements of generalized torque varies along the rows, while elements of v vary along the columns and elements of q along the pages of the tensor. Hence, d2tau_dqdv is essentially d (d tau/dq)/dv, with outer-most derivative representing the third dimension (pages) of the tensor. The tensor d2tau_dadq reduces down to dM/dq, and hence the elements of q vary along the pages of the tensor. In other words, this tensor derivative is d(d tau/da)/dq. All other remaining combinations of second-order derivatives of generalized torque are zero.

See also

Definition at line 158 of file rnea-second-order-derivatives.hpp.

ComputeRNEASecondOrderDerivatives() [2/2]

void pinocchio::ComputeRNEASecondOrderDerivatives ( 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, const Tensor1 &  d2tau_dqdq, const Tensor2 &  d2tau_dvdv, const Tensor3 &  dtau_dqdv, const Tensor4 &  dtau_dadq  )

inline

Computes the Second-Order partial derivatives of the Recursive Newton Euler Algorithm w.r.t the joint configuration, the joint velocity and the joint acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.
Tensor1	Type of the 3D-Tensor containing the SO partial derivative with respect to the joint configuration vector. The elements of Torque vector are along the 1st dim, and joint config along 2nd,3rd dimensions.
Tensor2	Type of the 3D-Tensor containing the Second-Order partial derivative with respect to the joint velocity vector. The elements of Torque vector are along the 1st dim, and the velocity along 2nd,3rd dimensions.
Tensor3	Type of the 3D-Tensor containing the cross Second-Order partial derivative with respect to the joint configuration and velocty vector. The elements of Torque vector are along the 1st dim, and the config. vector along 2nd dimension, and velocity along the third dimension.
Tensor4	Type of the 3D-Tensor containing the cross Second-Order partial derivative with respect to the joint configuration and acceleration vector. This is also the First-order partial derivative of Mass-Matrix (M) with respect to configuration vector. The elements of Torque vector are along the 1st dim, and the acceleration vector along 2nd dimension, while configuration along the third dimension.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[out]	d2tau_dqdq	Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration.
[out]	d2tau_dvdv	Second-Order Partial derivative of the generalized torque vector with respect to the joint velocity
[out]	dtau_dqdv	Cross Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration and velocity.
[out]	dtau_dadq	Cross Second-Order Partial derivative of the generalized torque vector with respect to the joint configuration and accleration.

Remarks

d2tau_dqdq, d2tau_dvdv, dtau_dqdv and dtau_dadq must be first initialized with zeros (d2tau_dqdq.setZero(), etc). The storage order of the 3D-tensor derivatives is important. For d2tau_dqdq, the elements of generalized torque varies along the rows, while elements of q vary along the columns and pages of the tensor. For dtau_dqdv, the elements of generalized torque varies along the rows, while elements of v vary along the columns and elements of q along the pages of the tensor. Hence, dtau_dqdv is essentially d (d tau/dq)/dv, with outer-most derivative representing the third dimension (pages) of the tensor. The tensor dtau_dadq reduces down to dM/dq, and hence the elements of q vary along the pages of the tensor. In other words, this tensor derivative is d(d tau/da)/dq. All other remaining combinations of second-order derivatives of generalized torque are zero.

See also

computeStaticRegressor()

DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x& pinocchio::computeStaticRegressor ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< ConfigVectorType > &  q  )

inline

Computes the static regressor that links the center of mass positions of all the links to the center of mass of the complete model according to the current configuration of the robot.

The result is stored in data.staticRegressor and it corresponds to a matrix \( Y \) such that \( c = Y(q,\dot{q},\ddot{q})\tilde{\pi} \) where \( \tilde{\pi} = (\tilde{\pi}_1^T\ \dots\ \tilde{\pi}_n^T)^T \) and \( \tilde{\pi}_i = \text{model.inertias[i].toDynamicParameters().head<4>()} \)

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).

Returns

The static regressor of the system.

computeStaticTorque()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::computeStaticTorque	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const std::vector< ForceTpl< Scalar, Options >> &	fext
	)

Computes the generalized static torque contribution \( g(q) - \sum J(q)^{\top} f_{\text{ext}} \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + c(q, \dot{q}) + g(q) = \tau + \sum J(q)^{\top} f_{\text{ext}} \end{eqnarray} \)

. This torque vector accouts for the contribution of the gravity and the external forces.

Note

This function is equivalent to pinocchio::rnea(model, data, q, 0, 0, fext).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).

Returns

The generalized static torque stored in data.tau.

computeStaticTorqueDerivatives()

void pinocchio::computeStaticTorqueDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const std::vector< ForceTpl< Scalar, Options >> &	fext,
		const Eigen::MatrixBase< ReturnMatrixType > &	static_torque_partial_dq
	)

Computes the partial derivative of the generalized gravity and external forces contributions (a.k.a static torque vector) with respect to the joint configuration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
ReturnMatrixType	Type of the matrix containing the partial derivative of the gravity vector with respect to the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	fext	External forces expressed in the local frame of the joints (dim model.njoints).
[out]	static_torque_partial_dq	Partial derivative of the static torque vector with respect to the joint configuration.

Remarks

gravity_partial_dq must be first initialized with zeros (gravity_partial_dq.setZero).

See also

pinocchio::computeGeneralizedTorque

computeSubtreeMasses()

void pinocchio::computeSubtreeMasses	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Compute the mass of each kinematic subtree and store it in data.mass. The element mass[0] corresponds to the total mass of the model.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Note

If you are only interested in knowing the total mass of the model, computeTotalMass will probably be slightly faster.

computeSupportedForceByFrame()

ForceTpl<Scalar, Options> pinocchio::computeSupportedForceByFrame	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id
	)

Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame. The supported force corresponds to the sum of all the forces experienced after the given frame, i.e :

• The inertial forces and gravity (applied on the supported inertia in body)
• The forces applied by child joints
• (The external forces) You must first call pinocchio::rnea to update placements, velocities and efforts values contained in the data structure.

Note

If an external force is applied to the frame parent joint (during rnea), it won't be taken in consideration in this function (it will be considered to be applied before the frame in the joint and not after. However external forces applied to child joints will be taken into account).

Physically speaking, if the robot were to be separated in two parts glued together at that given frame, the supported force represents the internal forces applide from the part after the cut/frame to the part before. This compute what a force-torque sensor would measures if it would be placed at that frame.

The equivalent function for a joint would be to read data.f[joint_id], after having call pinocchio::rnea.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	The index of the frame.

Returns

The computed force.

Warning

pinocchio::rnea should have been called first

computeSupportedInertiaByFrame()

InertiaTpl<Scalar, Options> pinocchio::computeSupportedInertiaByFrame	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		bool	with_subtree
	)

Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame. The total supported inertia corresponds to the sum of all the inertia after the given frame, i.e :

• The frame inertia
• The child frames inertia ('Child frames' refers to frames that share the same parent joint and are placed after the given frame)
• The child joints inertia (if with_subtree == true) You must first call pinocchio::forwardKinematics to update placement values in data structure.

Note

Physically speaking, if the robot were to be cut in two parts at that given frame, this supported inertia would represents the inertia of the part that was after the frame. with_subtree determines if the childs joints must be taken into consideration (if true) or only the current joint (if false).

The equivalent function for a joint would be :

• to read data.Ycrb[joint_id], after having called pinocchio::crba (if with_subtree == true).
• to read model.inertia[joint_id] (if with_subtree == false).

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	The index of the frame.
[in]	with_subtree	If false, compute the inertia only inside the frame parent joint if false. If true, include child joints inertia.

Returns

The computed inertia.

Warning

forwardKinematics should have been called first

computeTotalMass() [1/2]

Scalar pinocchio::computeTotalMass ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model )

inline

Compute the total mass of the model and return it.

Parameters

[in]

model

The model structure of the rigid body system.

Returns

Total mass of the model.

computeTotalMass() [2/2]

Scalar pinocchio::computeTotalMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Compute the total mass of the model, put it in data.mass[0] and return it.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Warning

This method does not fill the whole data.mass vector. Only data.mass[0] is updated. If you need the whole data.mass vector to be computed, use computeSubtreeMasses

Returns

Total mass of the model.

constrainedABA()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::constrainedABA ( 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 > &  tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &  contact_models, std::vector< ConstraintData, ConstraintDataAllocator > &  contact_datas, ProximalSettingsTpl< Scalar > &  settings  )

inline

The constrained Articulated Body Algorithm (constrainedABA). It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system. All the quantities are expressed in the LOCAL coordinate systems of the joint frames.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	contact_models	Vector of contact models.
[in]	contact_datas	Vector of contact data.
[in]	settings	Proximal settings (mu, accuracy and maximal number of iterations).

Note

A hint: a typical value of mu in proximal settings is 1e-6, and should always be positive. This also overwrites data.f, possibly leaving it in an inconsistent state.

Returns

A reference to the joint acceleration stored in data.ddq. data.lambdaA[0] stores the constraint forces.

constraintDynamics() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::constraintDynamics ( 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 > &  tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &  contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &  contact_datas  )

inline

Computes the forward dynamics with contact constraints according to a given list of contact information.

Note

When using forwardDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm. It computes the following problem:

\[ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \]

where \( \ddot{q}_{\text{free}} \) is the free acceleration (i.e. without constraints), \( M \) is the mass matrix, \( J \) the constraint Jacobian and \( \gamma \) is the constraint drift. By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (size model.nq).
[in]	v	The joint velocity (size model.nv).
[in]	tau	The joint torque vector (size model.nv).
[in]	contact_models	Vector of contact models.
[in]	contact_datas	Vector of contact data.

Returns

A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.

Definition at line 181 of file constrained-dynamics.hpp.

constraintDynamics() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::constraintDynamics ( 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 > &  tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &  contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &  contact_datas, ProximalSettingsTpl< Scalar > &  settings  )

inline

Computes the forward dynamics with contact constraints according to a given list of contact information.

Note

When using forwardDynamics for the first time, you should call first initConstraintDynamics to initialize the internal memory used in the algorithm. It computes the following problem:

\[ \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \]

where \( \ddot{q}_{\text{free}} \) is the free acceleration (i.e. without constraints), \( M \) is the mass matrix, \( J \) the constraint Jacobian and \( \gamma \) is the constraint drift. By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (size model.nq).
[in]	v	The joint velocity (size model.nv).
[in]	tau	The joint torque vector (size model.nv).
[in]	contact_models	Vector of contact models.
[in]	contact_datas	Vector of contact data.
[in]	settings	Proximal settings (mu, accuracy and maximal number of iterations).

Note

A hint: a typical value for mu is 1e-12 when two contact constraints are redundant.

Returns

A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.

contactInverseDynamics()

bool pinocchio::contactInverseDynamics	(	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,
		const Scalar	dt,
		const PointContactConstraintModelVector &	constraint_models,
		PointContactConstraintDataVector &	constraint_datas,
		const Eigen::MatrixBase< VectorLikeGamma > &	constraint_correction,
		const Eigen::MatrixBase< VectorLikeLam > &	_lambda_sol,
		ProximalSettingsTpl< Scalar > &	settings,
		bool	solve_ncp = true
	)

The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the presence of contacts, aka the joint torques according to the current state of the system and the desired joint accelerations.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[in]	dt	The time step.
[in]	constraint_models	The list of contact models.
[in]	constraint_datas	The list of constraint_datas.
[in]	constraint_correction	vector representing the constraint correction.
[in]	_lambda_sol	initial guess for the contact forces
[in]	settings	The settings for the proximal algorithm
[in]	solve_ncp	whether to solve the NCP (true) or CCP (false).

Returns

The desired joint torques stored in data.tau.

copy()

void pinocchio::copy ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  origin, DataTpl< Scalar, Options, JointCollectionTpl > &  dest, KinematicLevel  kinematic_level  )

inline

Copy part of the data from origin to dest. Template parameter can be used to select at which differential level the copy should occur.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	origin	Data from which the values are copied.
[out]	dest	Data to which the values are copied
[in]	kinematic_level	if =0, copy oMi. If =1, also copy v. If =2, also copy a, a_gf and f.

crba()

const DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs& pinocchio::crba	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Convention	convention = Convention::LOCAL
	)

Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid Body Algorithm (Chapter 6, Rigid-Body Dynamics Algorithms, R. Featherstone, 2008). The result is accessible through data.M.

Note

You can easily get data.M symmetric by copying the strictly upper triangular part in the strictly lower triangular part with data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

This algorithm also takes into account the rotor inertia effects, by adding on the diagonal of the Joint Space Inertia Matrix their contributions. This is done only for single DOF joint (e.g. Revolute, Prismatic, etc.).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Note

In WORLD convention, a direct outcome of this algorithm is the computation of the centroidal momentum matrix (data.Ag), a forward geometry and the joint jacobian matrix (data.J).

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	convention	Convention to use.

Returns

The joint space inertia matrix with only the upper triangular part computed.

dccrba()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x& pinocchio::dccrba	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.

Note

The computed terms allow to decomposed the spatial momentum variation as following: \( \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The Centroidal Momentum Matrix time derivative dAg (accessible via data.dAg).

Remarks

As another output, this algorithm also computes the Centroidal Momentum Matrix Ag (accessible via data.Ag), the Joint Jacobian matrix (accessible via data.J) and the time derivatibe of the Joint Jacobian matrix (accessible via data.dJ).

dDifference() [1/2]

void pinocchio::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
	)

Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity.

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 \( d(q0, q1) \) the difference function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q0: \( 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 \|)\).
• Jacobian relative to q1: \( 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 \|)\).

Parameters

[in]	model	Model of the kinematic tree on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Joint velocity (size model.nv)
[out]	J	Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv).
[in]	arg	Argument (either q0 or q1) with respect to which the differentiation is performed.

Definition at line 1037 of file joint-configuration.hpp.

dDifference() [2/2]

void pinocchio::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
	)

Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity.

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 \( d(q0, q1) \) the difference function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q0: \( 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 \|)\).
• Jacobian relative to q1: \( 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 \|)\).

Parameters

[in]	model	Model of the kinematic tree on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Joint velocity (size model.nv)
[out]	J	Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv).
[in]	arg	Argument (either q0 or q1) with respect to which the differentiation is performed.

Definition at line 1037 of file joint-configuration.hpp.

difference() [1/4]

ConfigVectorIn1 pinocchio::difference	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

Parameters

[in]	model	Model of the kinematic tree on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Finial configuration (size model.nq)

Returns

The corresponding velocity (size model.nv)

Parameters

[in]	model	Model of the kinematic tree on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Final configuration (size model.nq)

Returns

The corresponding velocity (size model.nv)

Definition at line 1574 of file joint-configuration.hpp.

difference() [2/4]

ConfigVectorIn1 pinocchio::difference	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

Parameters

[in]	model	Model of the kinematic tree on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Final configuration (size model.nq)

Returns

The corresponding velocity (size model.nv)

Definition at line 1574 of file joint-configuration.hpp.

difference() [3/4]

void pinocchio::difference	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Eigen::MatrixBase< ReturnType > &	dvout
	)

Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

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 \( q_1 \ominus q_0 \).

Parameters

[in]	model	Model of the system on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Desired configuration (size model.nq)
[out]	dvout	The corresponding velocity (size model.nv)

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 \( q_1 \ominus q_0 \).

Parameters

[in]	model	Model of the system on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Desired configuration (size model.nq)
[out]	dvout	The corresponding velocity (size model.nv).

Definition at line 230 of file joint-configuration.hpp.

difference() [4/4]

void pinocchio::difference	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Eigen::MatrixBase< ReturnType > &	dvout
	)

Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.

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 \( q_1 \ominus q_0 \).

Parameters

[in]	model	Model of the system on which the difference operation is performed.
[in]	q0	Initial configuration (size model.nq)
[in]	q1	Desired configuration (size model.nq)
[out]	dvout	The corresponding velocity (size model.nv).

Definition at line 230 of file joint-configuration.hpp.

dIntegrate() [1/3]

void pinocchio::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
	)

Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.

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(q, v) \) the integrate function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q: \( f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\).
• Jacobian relative to v: \( f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\).

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	J	Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
[in]	arg	Argument (either q or v) with respect to which the differentiation is performed.

Definition at line 484 of file joint-configuration.hpp.

dIntegrate() [2/3]

void pinocchio::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
	)

Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.

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(q, v) \) the integrate function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q: \( f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\).
• Jacobian relative to v: \( f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\).

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	J	Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
[in]	arg	Argument (either q or v) with respect to which the differentiation is performed.
[in]	op	Operation to apply.

Definition at line 530 of file joint-configuration.hpp.

dIntegrate() [3/3]

void pinocchio::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
	)

Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.

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(q, v) \) the integrate function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q: \( f(q \oplus \delta q, v) \ominus f(q, v) = J_q \delta q + o(\delta q)\).
• Jacobian relative to v: \( f(q, v + \delta v) \ominus f(q, v) = J_v \delta v + o(\delta v)\).

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	J	Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
[in]	arg	Argument (either q or v) with respect to which the differentiation is performed.
[in]	op	Operation to apply.

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(q, v) \) the integrate function, these jacobians satisfy the following relationships in the tangent space:

• Jacobian relative to q: \( f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\).
• Jacobian relative to v: \( f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\).

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	J	Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
[in]	arg	Argument (either q or v) with respect to which the differentiation is performed.
[in]	op	Operation to apply.

Definition at line 530 of file joint-configuration.hpp.

dIntegrateTransport() [1/4]

void pinocchio::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
	)

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.

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \( q \oplus v \), to the tangent space at \( q \). In other words, this functions transforms a tangent vector expressed at \( q \oplus v \) to a tangent vector expressed at \( q \), considering that the change of configuration between \( q \oplus v \) and \( q \) may alter the value of this tangent vector. A typical example of parallel transportation is the action operated by a rigid transformation \( M \in \text{SE}(3)\) on a spatial velocity \( v \in \text{se}(3)\). 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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[in,out]	J	Input/output matrix (number of rows = model.nv).
[in]	arg	Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed.

Definition at line 955 of file joint-configuration.hpp.

dIntegrateTransport() [2/4]

void pinocchio::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
	)

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.

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \( q \oplus v \), to the tangent space at \( q \). In other words, this functions transforms a tangent vector expressed at \( q \oplus v \) to a tangent vector expressed at \( q \), considering that the change of configuration between \( q \oplus v \) and \( q \) may alter the value of this tangent vector. A typical example of parallel transportation is the action operated by a rigid transformation \( M \in \text{SE}(3)\) on a spatial velocity \( v \in \text{se}(3)\). 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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[in,out]	J	Input/output matrix (number of rows = model.nv).
[in]	arg	Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed.

Definition at line 955 of file joint-configuration.hpp.

dIntegrateTransport() [3/4]

void pinocchio::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
	)

Transport a matrix from the terminal to the initial tangent space of the integrate operation, with respect to the configuration or the velocity arguments.

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \( q \oplus v \), to the tangent space at \( q \). In other words, this functions transforms a tangent vector expressed at \( q \oplus v \) to a tangent vector expressed at \( q \), considering that the change of configuration between \( q \oplus v \) and \( q \) may alter the value of this tangent vector. A typical example of parallel transportation is the action operated by a rigid transformation \( M \in \text{SE}(3)\) on a spatial velocity \( v \in \text{se}(3)\). 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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	Jin	Input matrix (number of rows = model.nv).
[out]	Jout	Output matrix (same size as Jin).
[in]	arg	Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed.

Definition at line 869 of file joint-configuration.hpp.

dIntegrateTransport() [4/4]

void pinocchio::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
	)

Transport a matrix from the terminal to the initial tangent space of the integrate operation, with respect to the configuration or the velocity arguments.

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space of the integrated element \( q \oplus v \), to the tangent space at \( q \). In other words, this functions transforms a tangent vector expressed at \( q \oplus v \) to a tangent vector expressed at \( q \), considering that the change of configuration between \( q \oplus v \) and \( q \) may alter the value of this tangent vector. A typical example of parallel transportation is the action operated by a rigid transformation \( M \in \text{SE}(3)\) on a spatial velocity \( v \in \text{se}(3)\). 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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	Jin	Input matrix (number of rows = model.nv).
[out]	Jout	Output matrix (same size as Jin).
[in]	arg	Argument (either ARG0 for q or ARG1 for v) with respect to which the differentation is performed.

Definition at line 869 of file joint-configuration.hpp.

distance() [1/2]

Scalar pinocchio::distance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2 \).

Parameters

[in]	model	Model we want to compute the distance
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The distance between the two configurations q0 and q1.

Distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2 \).

Parameters

[in]	model	Model we want to compute the distance
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The distance between the two configurations q0 and q1.

Definition at line 1142 of file joint-configuration.hpp.

distance() [2/2]

Scalar pinocchio::distance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Distance between two configuration vectors.

Distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2 \).

Parameters

[in]	model	Model we want to compute the distance
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The distance between the two configurations q0 and q1.

Definition at line 1142 of file joint-configuration.hpp.

findCommonAncestor() [1/2]

JointIndex pinocchio::findCommonAncestor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		JointIndex	joint1_id,
		JointIndex	joint2_id
	)

Computes the common ancestor between two joints belonging to the same kinematic tree.

Parameters

[in]	model	the input model.
[in]	joint1_id	index of the first joint.
[in]	joint2_id	index of the second joint.

Definition at line 300 of file model.hpp.

findCommonAncestor() [2/2]

JointIndex pinocchio::findCommonAncestor	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		JointIndex	joint1_id,
		JointIndex	joint2_id,
		size_t &	index_ancestor_in_support1,
		size_t &	index_ancestor_in_support2
	)

Computes the common ancestor between two joints belonging to the same kinematic tree.

Parameters

[in]	model	the input model.
[in]	joint1_id	index of the first joint.
[in]	joint2_id	index of the second joint.
[out]	index_ancestor_in_support1	index of the ancestor within model.supports[joint1_id].
[out]	index_ancestor_in_support2	index of the ancestor within model.supports[joint2_id].

forwardDynamics() [1/2]

PINOCCHIO_UNSUPPORTED const DataTpl<Scalar, Options, JointCollectionTpl>:: TangentVectorType& pinocchio::forwardDynamics ( 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 > &  tau, const Eigen::MatrixBase< ConstraintMatrixType > &  J, const Eigen::MatrixBase< DriftVectorType > &  gamma, const Scalar  inv_damping = 0.  )

inline

Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is called.

Warning

This function is no more supported. Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings]) instead.

Note

It solves the following problem:
\( \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\{} \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \)
where \( \ddot{q}_{\text{free}} \) is the free acceleration (i.e. without constraints), \( M \) is the mass matrix, \( J \) the constraint Jacobian and \( \gamma \) is the constraint drift. By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
ConstraintMatrixType	Type of the constraint matrix.
DriftVectorType	Type of the drift vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	v	The joint velocity (vector dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	J	The Jacobian of the constraints (dim nb_constraints*model.nv).
[in]	gamma	The drift of the constraints (dim nb_constraints).
[in]	inv_damping	Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.

Note

A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.

Returns

A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.

forwardDynamics() [2/2]

PINOCCHIO_UNSUPPORTED const DataTpl<Scalar, Options, JointCollectionTpl>:: TangentVectorType& pinocchio::forwardDynamics ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< TangentVectorType > &  tau, const Eigen::MatrixBase< ConstraintMatrixType > &  J, const Eigen::MatrixBase< DriftVectorType > &  gamma, const Scalar  inv_damping = 0.  )

inline

Compute the forward dynamics with contact constraints, assuming pinocchio::computeAllTerms has been called.

Warning

This function is no more supported. Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and constraintDynamics(model, data, q, v, tau, contact_models, contact_datas[,prox_settings]) instead.

Note

It solves the following problem:
\( \begin{eqnarray} \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\{} \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 \end{eqnarray} \)
where \( \ddot{q}_{\text{free}} \) is the free acceleration (i.e. without constraints), \( M \) is the mass matrix, \( J \) the constraint Jacobian and \( \gamma \) is the constraint drift. By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse is performed.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
ConstraintMatrixType	Type of the constraint matrix.
DriftVectorType	Type of the drift vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	tau	The joint torque vector (dim model.nv).
[in]	J	The Jacobian of the constraints (dim nb_constraints*model.nv).
[in]	gamma	The drift of the constraints (dim nb_constraints).
[in]	inv_damping	Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.

Note

A hint: 1e-12 as the damping factor gave good result in the particular case of redundancy in contact constraints on the two feet.

Returns

A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.

forwardKinematics() [1/3]

void pinocchio::forwardKinematics	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q
	)

Update the joint placements according to the current joint configuration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).

forwardKinematics() [2/3]

void pinocchio::forwardKinematics	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Update the joint placements and spatial velocities according to the current joint configuration and velocity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	v	The joint velocity (vector dim model.nv).

forwardKinematics() [3/3]

void pinocchio::forwardKinematics	(	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
	)

Update the joint placements, spatial velocities and spatial accelerations according to the current joint configuration, velocity and acceleration.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	v	The joint velocity (vector dim model.nv).
[in]	a	The joint acceleration (vector dim model.nv).

frameBodyRegressor()

DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType& pinocchio::frameBodyRegressor ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, FrameIndex  frame_id  )

inline

Computes the regressor for the dynamic parameters of a rigid body attached to a given frame, puts the result in data.bodyRegressor and returns it.

This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.

The result is such that \( f = \text{frameBodyRegressor(model,data,frame_id) * I.toDynamicParameters()} \) where \( f \) is the net force acting on the body, including gravity

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	The id of the frame.

Returns

The dynamic regressor of the body.

framesForwardKinematics()

void pinocchio::framesForwardKinematics ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< ConfigVectorType > &  q  )

inline

First calls the forwardKinematics on the model, then computes the placement of each frame. /sa pinocchio::forwardKinematics.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The kinematic model.
	data	Data associated to model.
[in]	q	Configuration vector.

getAcceleration()

MotionTpl<Scalar, Options> pinocchio::getAcceleration	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	jointId,
		const ReferenceFrame	rf = LOCAL
	)

Returns the spatial acceleration of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	jointId	Id of the joint
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The spatial acceleration of the joint expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

getCenterOfMassVelocityDerivatives()

void pinocchio::getCenterOfMassVelocityDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< Matrix3xOut > &	vcom_partial_dq
	)

Computes the partial derivatie of the center-of-mass velocity with respect to the joint configuration q. You must first call computeAllTerms(model,data,q,v) or computeCenterOfMass(model,data,q,v) before calling this function.

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xOut	Matrix3x containing the partial derivatives of the CoM velocity with respect to the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[out]	vcom_partial_dq	Partial derivative of the CoM velocity w.r.t. \( q \).

getCentroidalDynamicsDerivatives()

void pinocchio::getCentroidalDynamicsDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< Matrix6xLike1 > &	dh_dq,
		const Eigen::MatrixBase< Matrix6xLike1 > &	dhdot_dq,
		const Eigen::MatrixBase< Matrix6xLike2 > &	dhdot_dv,
		const Eigen::MatrixBase< Matrix6xLike3 > &	dhdot_da
	)

Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives. pinocchio::computeRNEADerivatives should have been called first.

Computes the first order approximation of the centroidal dynamics time derivative and corresponds to the following equation \( d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q} + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v} + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a} \)

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[out]	dh_dq	The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv). ///
[out]	dhdot_dq	The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).
[out]	dhdot_dv	The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).
[out]	dhdot_da	The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).

Note

It also computes the current centroidal dynamics and its time derivative. For information, the centroidal momentum matrix is equivalent to dhdot_da.

getClassicalAcceleration()

MotionTpl<Scalar, Options> pinocchio::getClassicalAcceleration	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	jointId,
		const ReferenceFrame	rf = LOCAL
	)

Returns the "classical" acceleration of the joint expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	jointId	Id of the joint
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The classic acceleration of the joint expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

getComFromCrba()

const DataTpl<Scalar, Options, JointCollectionTpl>::Vector3& pinocchio::getComFromCrba	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Extracts the center of mass position from the joint space inertia matrix (also called the mass matrix).

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xLike	Type of the output Jacobian matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The center of mass position of the rigid body system expressed in the world frame (vector 3).

getCoriolisMatrix()

const DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs& pinocchio::getCoriolisMatrix	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Retrives the Coriolis Matrix \( C(q,\dot{q}) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau \end{eqnarray} \)

after a call to the dynamics derivatives.

Note

In the previous equation, \( c(q, \dot{q}) = C(q, \dot{q})\dot{q} \).

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The Coriolis matrix stored in data.C.

getFrameAcceleration() [1/2]

MotionTpl<Scalar, Options> pinocchio::getFrameAcceleration ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const FrameIndex  frame_id, const ReferenceFrame  rf = LOCAL  )

inline

Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in the data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The spatial acceleration of the Frame expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

Remarks

In the context of a frame placement constraint \(J(q) a + \dot{J}(q, v) v = 0\), one way to compute the second term \(\dot{J}(q, v) v\) is to call second-order forwardKinematics with a zero acceleration, then read the remaining \(\dot{J}(q, v) v\) by calling this function. This is significantly more efficient than applying the matrix \(\dot{J}(q, v)\) (from getFrameJacobianTimeVariation) to the velocity vector \(v\).

Definition at line 185 of file frames.hpp.

getFrameAcceleration() [2/2]

MotionTpl<Scalar, Options> pinocchio::getFrameAcceleration ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const JointIndex  joint_id, const SE3Tpl< Scalar, Options > &  placement, const ReferenceFrame  rf = LOCAL  )

inline

Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Id of the parent joint
[in]	placement	frame placement with respect to the parent joint
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The spatial acceleration of the Frame expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

getFrameAccelerationDerivatives() [1/4]

void pinocchio::getFrameAccelerationDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		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
	)

Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	a_partial_dq	Partial derivative of the frame spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the frame spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the frame spatial acceleration w.r.t. \( \dot{v} \).

Definition at line 201 of file frames-derivatives.hpp.

getFrameAccelerationDerivatives() [2/4]

void pinocchio::getFrameAccelerationDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		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
	)

Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
Matrix6xOut5	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the frame spatial velociy w.r.t. \( v \).
[out]	a_partial_dq	Partial derivative of the frame spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the frame spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the frame spatial acceleration w.r.t. \( \dot{v} \).

Definition at line 329 of file frames-derivatives.hpp.

getFrameAccelerationDerivatives() [3/4]

void pinocchio::getFrameAccelerationDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const SE3Tpl< Scalar, Options > &	placement,
		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
	)

Computes the partial derivatives of the spatial acceleration of a frame given by its relative placement, with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Index of the supporting joint
[in]	placement	Placement of the Frame w.r.t. the joint frame.
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	a_partial_dq	Partial derivative of the frame spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the frame spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the frame spatial acceleration w.r.t. \( \dot{v} \).

getFrameAccelerationDerivatives() [4/4]

void pinocchio::getFrameAccelerationDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const SE3Tpl< Scalar, Options > &	placement,
		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
	)

Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities. 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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
Matrix6xOut5	Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Index of the supporting joint
[in]	placement	Placement of the Frame w.r.t. the joint frame.
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the frame spatial velociy w.r.t. \( v \).
[out]	a_partial_dq	Partial derivative of the frame spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the frame spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the frame spatial acceleration w.r.t. \( \dot{v} \).

Definition at line 269 of file frames-derivatives.hpp.

getFrameClassicalAcceleration() [1/2]

MotionTpl<Scalar, Options> pinocchio::getFrameClassicalAcceleration ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const FrameIndex  frame_id, const ReferenceFrame  rf = LOCAL  )

inline

Returns the "classical" acceleration of the Frame expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The classical acceleration of the Frame expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

Remarks

In the context of a frame placement constraint \(J(q) a + \dot{J}(q, v) v = 0\), one way to compute the second term \(\dot{J}(q, v) v\) is to call second-order forwardKinematics with a zero acceleration, then read the remaining \(\dot{J}(q, v) v\) by calling this function. This is significantly more efficient than applying the matrix \(\dot{J}(q, v)\) (from getFrameJacobianTimeVariation) to the velocity vector \(v\).

Definition at line 245 of file frames.hpp.

getFrameClassicalAcceleration() [2/2]

MotionTpl<Scalar, Options> pinocchio::getFrameClassicalAcceleration ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const JointIndex  joint_id, const SE3Tpl< Scalar, Options > &  placement, const ReferenceFrame  rf = LOCAL  )

inline

Returns the "classical" acceleration of the Frame expressed in the desired reference frame. This is different from the "spatial" acceleration in that centrifugal effects are accounted for. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Id of the parent joint
[in]	placement	frame placement with respect to the parent joint
[in]	rf	Reference frame in which the acceleration is expressed.

Returns

The classical acceleration of the Frame expressed in the desired reference frame.

Warning

Second order forwardKinematics should have been called first

getFrameJacobian() [1/4]

Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> pinocchio::getFrameJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		const ReferenceFrame	reference_frame
	)

Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians.

Remarks

Similarly to pinocchio::getJointJacobian:

• if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame
• if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin and expressed in a coordinate system aligned with the WORLD.
• if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin and expressed in a coordinate system aligned with the WORLD.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Index of the frame
[in]	reference_frame	Reference frame in which the Jacobian is expressed.

Warning

The function pinocchio::computeJointJacobians should have been called first.

Definition at line 416 of file frames.hpp.

getFrameJacobian() [2/4]

void pinocchio::getFrameJacobian ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const FrameIndex  frame_id, const ReferenceFrame  reference_frame, const Eigen::MatrixBase< Matrix6xLike > &  J  )

inline

Returns the jacobian of the frame expressed either expressed in the local frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians.

Remarks

Similarly to pinocchio::getJointJacobian:

• if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame
• if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin and expressed in a coordinate system aligned with the WORLD.
• if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin and expressed in a coordinate system aligned with the WORLD.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Index of the frame
[in]	reference_frame	Reference frame in which the Jacobian is expressed.
[out]	J	The Jacobian of the Frame expressed in the coordinates Frame.

Warning

The function pinocchio::computeJointJacobians should have been called first.

Definition at line 370 of file frames.hpp.

getFrameJacobian() [3/4]

Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> pinocchio::getFrameJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const SE3Tpl< Scalar, Options > &	placement,
		const ReferenceFrame	reference_frame
	)

Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians.

Remarks

Similarly to pinocchio::getJointJacobian:

• if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame
• if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin and expressed in a coordinate system aligned with the WORLD.
• if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin and expressed in a coordinate system aligned with the WORLD.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Index of the joint.
[in]	placement	Frame placement with respect to the parent joint
[in]	reference_frame	Reference frame in which the Jacobian is expressed.

Warning

The function pinocchio::computeJointJacobians should have been called first.

Definition at line 324 of file frames.hpp.

getFrameJacobian() [4/4]

void pinocchio::getFrameJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const SE3Tpl< Scalar, Options > &	placement,
		const ReferenceFrame	reference_frame,
		const Eigen::MatrixBase< Matrix6xLike > &	J
	)

Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame, and whose columns are either expressed in the LOCAL frame coordinate system, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the WORLD coordinate system, depending on the value of reference_frame. You must first call pinocchio::computeJointJacobians.

Remarks

Similarly to pinocchio::getJointJacobian:

• if rf == LOCAL, this function returns the Jacobian of the frame expressed in the local coordinate system of the frame
• if rf == LOCAL_WORLD_ALIGNED, this function returns the Jacobian of the frame centered on the frame origin and expressed in a coordinate system aligned with the WORLD.
• if rf == WORLD, this function returns the Jacobian of the frame expressed at the point coincident with the origin and expressed in a coordinate system aligned with the WORLD.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Index of the joint.
[in]	placement	Frame placement with respect to the parent joint
[in]	reference_frame	Reference frame in which the Jacobian is expressed.
[out]	J	The Jacobian of the Frame expressed in the reference_frame coordinate system.

Warning

The function pinocchio::computeJointJacobians should have been called first.

getFrameJacobianTimeVariation()

void pinocchio::getFrameJacobianTimeVariation	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		const ReferenceFrame	rf,
		const Eigen::MatrixBase< Matrix6xLike > &	dJ
	)

Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the WORLD frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the LOCAL frame (rf = LOCAL).

Note

This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	The index of the frame.
[in]	rf	Reference frame in which the velocity is expressed.
[out]	dJ	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).

getFrameKinematicHessian() [1/4]

Tensor<Scalar, 3, Options> pinocchio::getFrameKinematicHessian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Model::FrameIndex	frame_id,
		const ReferenceFrame	rf
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	Index of the frame in model.
[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. \( q \) expressed in the frame given by rf.

Remarks

This function is also related to

See also

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.

Definition at line 655 of file kinematics-derivatives.hpp.

getFrameKinematicHessian() [2/4]

void pinocchio::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
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	frame_id	Index of the frame in model.
[in]	rf	Reference frame with respect to which the derivative of the Jacobian is expressed
[out]	kinematic_hessian	Second order derivative of the joint placement w.r.t. \( q \) expressed in the frame given by rf.

Remarks

This function is also related to

See also

computeJointKinematicHessians. kinematic_hessian has to be initialized with zero when calling this function for the first time and there is no dynamic memory allocation.

Definition at line 527 of file kinematics-derivatives.hpp.

getFrameKinematicHessian() [3/4]

Tensor<Scalar, 3, Options> pinocchio::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
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	frame_placement	frame placement with respect to the parent joint
[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. \( q \) expressed in the frame given by rf.

Remarks

This function is also related to

See also

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.

Definition at line 613 of file kinematics-derivatives.hpp.

getFrameKinematicHessian() [4/4]

void pinocchio::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
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	frame_placement	frame placement with respect to the parent joint
[in]	rf	Reference frame with respect to which the derivative of the Jacobian is expressed
[out]	kinematic_hessian	Second order derivative of the joint placement w.r.t. \( q \) expressed in the frame given by rf.

Remarks

This function is also related to

See also

computeJointKinematicHessians. kinematic_hessian has to be initialized with zero when calling this function for the first time and there is no dynamic memory allocation.

getFrameVelocity() [1/2]

MotionTpl<Scalar, Options> pinocchio::getFrameVelocity ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const FrameIndex  frame_id, const ReferenceFrame  rf = LOCAL  )

inline

Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the velocity is expressed.

Returns

The spatial velocity of the Frame expressed in the desired reference frame.

Warning

Fist or second order forwardKinematics should have been called first

Definition at line 127 of file frames.hpp.

getFrameVelocity() [2/2]

MotionTpl<Scalar, Options> pinocchio::getFrameVelocity ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const JointIndex  joint_id, const SE3Tpl< Scalar, Options > &  placement, const ReferenceFrame  rf = LOCAL  )

inline

Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Id of the parent joint
[in]	placement	frame placement with respect to the parent joint
[in]	rf	Reference frame in which the velocity is expressed.

Returns

The spatial velocity of the Frame expressed in the desired reference frame.

Warning

Fist or second order forwardKinematics should have been called first

getFrameVelocityDerivatives() [1/2]

void pinocchio::getFrameVelocityDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const SE3Tpl< Scalar, Options > &	placement,
		const ReferenceFrame	rf,
		const Eigen::MatrixBase< Matrix6xOut1 > &	v_partial_dq,
		const Eigen::MatrixBase< Matrix6xOut2 > &	v_partial_dv
	)

Computes the partial derivatives of the spatial velocity of a frame given by its relative placement, with respect to q and v. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	joint_id	Index of the supporting joint
[in]	placement	Placement of the Frame w.r.t. the joint frame.
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the frame spatial velociy w.r.t. \( v \).

getFrameVelocityDerivatives() [2/2]

void pinocchio::getFrameVelocityDerivatives	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const FrameIndex	frame_id,
		const ReferenceFrame	rf,
		const Eigen::MatrixBase< Matrix6xOut1 > &	v_partial_dq,
		const Eigen::MatrixBase< Matrix6xOut2 > &	v_partial_dv
	)

Computes the partial derivatives of the frame spatial velocity with respect to q and v. You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	frame_id	Id of the operational Frame
[in]	rf	Reference frame in which the velocity is expressed.
[out]	v_partial_dq	Partial derivative of the frame spatial velocity w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the frame spatial velociy w.r.t. \( v \).

Definition at line 94 of file frames-derivatives.hpp.

getJacobianComFromCrba()

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x& pinocchio::getJacobianComFromCrba	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Extracts both the jacobian of the center of mass (CoM), the total mass of the system and the CoM position from the joint space inertia matrix (also called the mass matrix). The results are accessible through data.Jcom, data.mass[0] and data.com[0] and are both expressed in the world frame.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Returns

The jacobian of the CoM expressed in the world frame (matrix 3 x model.nv).

Remarks

This extraction of inertial quantities is only valid for free-floating base systems.

getJacobianSubtreeCenterOfMass()

void pinocchio::getJacobianSubtreeCenterOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex &	rootSubtreeId,
		const Eigen::MatrixBase< Matrix3xLike > &	res
	)

Retrieves the Jacobian of the center of mass of the given subtree according to the current value stored in data. It assumes that pinocchio::jacobianCenterOfMass has been called first with computeSubtreeComs equals to true.

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xLike	Type of the output Jacobian matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	rootSubtreeId	Index of the parent joint supporting the subtree.
[out]	res	The Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must first fill J with zero elements, e.g. J.setZero().

getJointAccelerationDerivatives() [1/2]

void pinocchio::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
	)

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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	jointId	Index of the joint in model.
[in]	rf	Reference frame in which the Jacobian is expressed.
[out]	v_partial_dq	Partial derivative of the joint spatial velocity w.r.t. \( q \).
[out]	a_partial_dq	Partial derivative of the joint spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the joint spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the joint spatial acceleration w.r.t. \( \dot{v} \).

getJointAccelerationDerivatives() [2/2]

void pinocchio::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
	)

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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector.
Matrix6xOut3	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector.
Matrix6xOut4	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector.
Matrix6xOut5	Matrix6x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	jointId	Index of the joint in model.
[in]	rf	Reference frame in which the Jacobian is expressed.
[out]	v_partial_dq	Partial derivative of the joint spatial velocity w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the joint spatial velociy w.r.t. \( v \).
[out]	a_partial_dq	Partial derivative of the joint spatial acceleration w.r.t. \( q \).
[out]	a_partial_dv	Partial derivative of the joint spatial acceleration w.r.t. \( v \).
[out]	a_partial_da	Partial derivative of the joint spatial acceleration w.r.t. \( \dot{v} \).

getJointJacobian() [1/2]

Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> pinocchio::getJointJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	reference_frame
	)

Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame, in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint.

Note

This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians before calling it.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	The index of the joint.
[in]	reference_frame	Reference frame in which the result is expressed.

Definition at line 148 of file jacobian.hpp.

getJointJacobian() [2/2]

void pinocchio::getJointJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	reference_frame,
		const Eigen::MatrixBase< Matrix6Like > &	J
	)

Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame options.

For the LOCAL reference frame, the Jacobian \({}^j J_{0j}\) from the joint frame \(j\) to the world frame \(0\) is such that \({}^j v_{0j} = {}^j J_{0j} \dot{q}\), where \({}^j v_{0j}\) is the velocity of the origin of the moving joint frame relative to the fixed world frame, projected into the basis of the joint frame. LOCAL_WORLD_ALIGNED is the same velocity but projected into the world frame basis.

For the WORLD reference frame, the Jacobian \({}^0 J_{0j}\) from the joint frame \(j\) to the world frame \(0\) is such that \({}^0 v_{0j} = {}^0 J_{0j} \dot{q}\), where \({}^0 v_{0j}\) is the spatial velocity of the joint frame. The linear component of this spatial velocity is the velocity of a (possibly imaginary) point attached to the moving joint frame j which is traveling through the origin of the world frame at that instant. The angular component is the instantaneous angular velocity of the joint frame as viewed in the world frame.

When serialized to a 6D vector, the order of coordinates is: three linear followed by three angular.

For further details regarding the different velocities or the Jacobian see Chapters 2 and 3 respectively in A Mathematical Introduction to Robotic Manipulation by Murray, Li and Sastry.

Note

This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians before calling it.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	The id of the joint.
[in]	reference_frame	Reference frame in which the result is expressed.
[out]	J	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).

getJointJacobianTimeVariation()

void pinocchio::getJointJacobianTimeVariation	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	joint_id,
		const ReferenceFrame	reference_frame,
		const Eigen::MatrixBase< Matrix6Like > &	dJ
	)

Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (rf = WORLD), in the local world aligned (rf = LOCAL_WORLD_ALIGNED) frame or in the local frame (rf = LOCAL) of the joint.

Note

This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xLike	Type of the matrix containing the joint Jacobian.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	The id of the joint.
[in]	reference_frame	Reference frame in which the result is expressed.
[out]	dJ	A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).

getJointKinematicHessian() [1/2]

Tensor<Scalar, 3, Options> pinocchio::getJointKinematicHessian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Model::JointIndex	joint_id,
		const ReferenceFrame	rf
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[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 also

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.

Definition at line 571 of file kinematics-derivatives.hpp.

getJointKinematicHessian() [2/2]

void pinocchio::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
	)

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 \( q \), 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.

Template Parameters

Scalar	Scalar type of the kinematic model.
Options	Alignement options of the kinematic model.
JointCollection	Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	rf	Reference frame with respect to which the derivative of the Jacobian is expressed
[out]	kinematic_hessian	Second order derivative of the joint placement w.r.t. \( q \) expressed in the frame given by rf.

Remarks

This function is also related to

See also

computeJointKinematicHessians. kinematic_hessian has to be initialized with zero when calling this function for the first time and there is no dynamic memory allocation.

getJointVelocityDerivatives()

void pinocchio::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
	)

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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix6xOut1	Matrix6x containing the partial derivatives with respect to the joint configuration vector.
Matrix6xOut2	Matrix6x containing the partial derivatives with respect to the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	jointId	Index of the joint in model.
[in]	rf	Reference frame in which the Jacobian is expressed.
[out]	v_partial_dq	Partial derivative of the joint velociy w.r.t. \( q \).
[out]	v_partial_dv	Partial derivative of the joint velociy w.r.t. \( v \).

getKKTContactDynamicMatrixInverse()

PINOCCHIO_UNSUPPORTED void pinocchio::getKKTContactDynamicMatrixInverse	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConstraintMatrixType > &	J,
		const Eigen::MatrixBase< KKTMatrixType > &	KKTMatrix_inv
	)

Computes the inverse of the KKT matrix for dynamics with contact constraints.

Warning

forwardDynamics/impuseDynamics are unsupported algorithm, and this function signature needs forwardDynamics to be called before. Please use the inverse() function in ConstraintCholeskyDecomposition class instead. It computes the following matrix:
\( \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1} \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \)

Remarks

The matrix is defined when one's call forwardDynamics/impulseDynamics. This method makes use of the matrix decompositions performed during the forwardDynamics/impulseDynamics and returns the inverse. The jacobian should be the same that the one provided to forwardDynamics/impulseDynamics. Thus forward Dynamics/impulseDynamics should have been called first.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	J	Jacobian of the constraints.
[out]	KKTMatrix_inv	inverse of the MJtJ matrix.

getPointClassicAccelerationDerivatives() [1/2]

void pinocchio::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
	)

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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xOut1	Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector.
Matrix3xOut2	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector.
Matrix3xOut3	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector.
Matrix3xOut4	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	placement	Relative placement of the point w.r.t. the joint frame.
[in]	rf	Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED).
[out]	v_point_partial_dq	Partial derivative of the point velocity w.r.t. \( q \).
[out]	a_point_partial_dq	Partial derivative of the point classic acceleration w.r.t. \( q \).
[out]	a_point_partial_dv	Partial derivative of the point classic acceleration w.r.t. \( v \).
[out]	a_point_partial_da	Partial derivative of the point classic acceleration w.r.t. \( \dot{v} \).

getPointClassicAccelerationDerivatives() [2/2]

void pinocchio::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
	)

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..

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xOut1	Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector.
Matrix3xOut2	Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector.
Matrix3xOut3	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint configuration vector.
Matrix3xOut4	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint velocity vector.
Matrix3xOut5	Matrix3x containing the partial derivatives of the spatial acceleration with respect to the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	placement	Relative placement of the point w.r.t. the joint frame.
[in]	rf	Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED).
[out]	v_point_partial_dq	Partial derivative of the point velocity w.r.t. \( q \).
[out]	v_point_partial_dv	Partial derivative of the point velociy w.r.t. \( v \).
[out]	a_point_partial_dq	Partial derivative of the point classic acceleration w.r.t. \( q \).
[out]	a_point_partial_dv	Partial derivative of the point classic acceleration w.r.t. \( v \).
[out]	a_point_partial_da	Partial derivative of the point classic acceleration w.r.t. \( \dot{v} \).

getPointVelocityDerivatives()

void pinocchio::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
	)

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.

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xOut1	Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint configuration vector.
Matrix3xOut2	Matrix3x containing the partial derivatives of the spatial velocity with respect to the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	Index of the joint in model.
[in]	placement	Relative placement of the point w.r.t. the joint frame.
[in]	rf	Reference frame in which the Jacobian is expressed (either LOCAL or LOCAL_WORLD_ALIGNED).
[out]	v_point_partial_dq	Partial derivative of the point velocity w.r.t. \( q \).
[out]	v_point_partial_dv	Partial derivative of the point velociy w.r.t. \( v \).

getRelativePlacement()

SE3Tpl<Scalar, Options> pinocchio::getRelativePlacement	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	jointIdRef,
		const JointIndex	jointIdTarget,
		const Convention	convention = Convention::LOCAL
	)

Returns the relative placement of two joints expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement values in data structure. LOCAL convention should only be used when aba and crba algorithms are called in LOCAL convention as well.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	jointIdRef	Id of the reference joint
[in]	jointIdTarget	Id of the target joint
[in]	convention	Convention to use (computation is done using data.liMi if LOCAL, and data.oMi if WORLD).

Returns

The relative placement of the target joint wrt to the refence joint, expressed in the desired reference frame.

Note

WORLD convention complexity is in O(1) and LOCAL is in O(n).

getTangentToConfigurationSparsitySegment()

void pinocchio::getTangentToConfigurationSparsitySegment	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const std::vector< JointIndex > &	joint_ids,
		std::vector< int > &	nvs,
		std::vector< int > &	idx_vs
	)

Return two vector where for each, the idx_v and v associated to the same atomic joint is given.

Parameters

[in]	model	Model of the kinematic tree.
[in]	joint_ids	Joint to condider to compute the tangentMap
[out]	nvs	For each id give the nv of the associated joint
[out]	idx_vs	For each id give the idx_v of the associated joint

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.

getVelocity()

MotionTpl<Scalar, Options> pinocchio::getVelocity	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex	jointId,
		const ReferenceFrame	rf = LOCAL
	)

Returns the spatial velocity of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.

Parameters

[in]	model	The kinematic model
[in]	data	Data associated to model
[in]	jointId	Id of the joint
[in]	rf	Reference frame in which the velocity is expressed.

Returns

The spatial velocity of the joint expressed in the desired reference frame.

Warning

Fist or second order forwardKinematics should have been called first

impulseDynamics() [1/3]

PINOCCHIO_UNSUPPORTED const DataTpl<Scalar, Options, JointCollectionTpl>:: TangentVectorType& pinocchio::impulseDynamics ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< ConfigVectorType > &  q, const Eigen::MatrixBase< TangentVectorType > &  v_before, const Eigen::MatrixBase< ConstraintMatrixType > &  J, const Scalar  r_coeff = 0., const Scalar  inv_damping = 0.  )

inline

Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.

Warning

This function is no more supported. Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu]) instead.

Note

It solves the following problem:
\( \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\{} \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \)
where \( \dot{q}^{-} \) is the generalized velocity before impact, \( M \) is the joint space mass matrix, \( J \) the constraint Jacobian and \( \epsilon \) is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.
ConstraintMatrixType	Type of the constraint matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (vector dim model.nq).
[in]	v_before	The joint velocity before impact (vector dim model.nv).
[in]	J	The Jacobian of the constraints (dim nb_constraints*model.nv).
[in]	r_coeff	The coefficient of restitution. Must be in [0;1].
[in]	inv_damping	Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.

Returns

A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.

impulseDynamics() [2/3]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::impulseDynamics	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType1 > &	v_before,
		const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &	contact_models,
		std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &	contact_datas,
		const Scalar	r_coeff,
		const ProximalSettingsTpl< Scalar > &	settings
	)

Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.

It computes the following problem:

\[ \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\ \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \]

where \( \dot{q}^{-} \) is the generalized velocity before impact, \( M \) is the joint space mass matrix, \( J \) the constraint Jacobian and \( \epsilon \) is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration (size model.nq).
[in]	v_before	The joint velocity (size model.nv).
[in]	contact_models	Vector of contact information related to the problem.
[in]	contact_datas	Vector of contact datas related to the contact models.
[in]	r_coeff	coefficient of restitution: must be in [0.,1.].
[in]	settings	Proximal solver settings.

Note

A hint: a typical value for mu is 1e-12 when two contact constraints are redundant.

Returns

A reference to the joint velocities stored in data.dq_after. The Lagrange Multipliers linked to the contact forces are available throw data.impulse_c vector.

impulseDynamics() [3/3]

PINOCCHIO_UNSUPPORTED const DataTpl<Scalar, Options, JointCollectionTpl>:: TangentVectorType& pinocchio::impulseDynamics ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const Eigen::MatrixBase< TangentVectorType > &  v_before, const Eigen::MatrixBase< ConstraintMatrixType > &  J, const Scalar  r_coeff = 0., const Scalar  inv_damping = 0.  )

inline

Compute the impulse dynamics with contact constraints, assuming pinocchio::crba has been called.

Warning

This function is no more supported. Please use the class RigidConstraintModel to define new contacts, and initConstraintDynamics(model, data, contact_models) and impulseDynamics(model, data, q, v_before, contact_models, contact_datas[,r_coeff, mu]) instead.

Note

It solves the following problem:
\( \begin{eqnarray} \underset{\dot{q}^{+}}{\min} & & \| \dot{q}^{+} - \dot{q}^{-} \|_{M(q)} \\{} \text{s.t.} & & J (q) \dot{q}^{+} = - \epsilon J (q) \dot{q}^{-} \end{eqnarray} \)
where \( \dot{q}^{-} \) is the generalized velocity before impact, \( M \) is the joint space mass matrix, \( J \) the constraint Jacobian and \( \epsilon \) is the coefficient of restitution (1 for a fully elastic impact or 0 for a rigid impact).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.
ConstraintMatrixType	Type of the constraint matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	v_before	The joint velocity before impact (vector dim model.nv).
[in]	J	The Jacobian of the constraints (dim nb_constraints*model.nv).
[in]	r_coeff	The coefficient of restitution. Must be in [0;1].
[in]	inv_damping	Damping factor for Cholesky decomposition of JMinvJt. Set to zero if constraints are full rank.

Returns

A reference to the generalized velocity after impact stored in data.dq_after. The Lagrange Multipliers linked to the contact impulsed are available throw data.impulse_c vector.

initConstraintDynamics()

void pinocchio::initConstraintDynamics ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const std::vector< ConstraintModel, ConstraintModelAllocator > &  contact_models, const std::vector< ConstraintData, ConstraintDataAllocator > &  contact_datas  )

inline

Init the forward dynamics data according to the contact information contained in contact_models.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	contact_models	Vector of contact information related to the problem.
[in]	contact_datas	Vector of contact data.

initPvSolver()

void pinocchio::initPvSolver ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const std::vector< ConstraintModel, ConstraintModelAllocator > &  contact_models  )

inline

Init the data according to the contact information contained in contact_models.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	contact_models	Vector of contact information related to the problem.

integrate() [1/4]

ConfigVectorType pinocchio::integrate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Integrate a configuration vector for the specified model for a tangent vector during one unit time.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)

Returns

The integrated configuration (size model.nq)

Definition at line 1464 of file joint-configuration.hpp.

integrate() [2/4]

ConfigVectorType pinocchio::integrate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Integrate a configuration vector for the specified model for a tangent vector during one unit time.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)

Returns

The integrated configuration (size model.nq)

Definition at line 1464 of file joint-configuration.hpp.

integrate() [3/4]

void pinocchio::integrate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

Integrate a configuration vector for the specified model for a tangent vector during one unit time.

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 \( q \oplus v \).

Parameters

[in]	model	Model of the kinematic tree on which the integration is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	qout	The integrated configuration (size model.nq)

Definition at line 105 of file joint-configuration.hpp.

integrate() [4/4]

void pinocchio::integrate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

Integrate a configuration vector for the specified model for a tangent vector during one unit time.

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 \( q \oplus v \).

Parameters

[in]	model	Model of the kinematic tree on which the integration is performed.
[in]	q	Initial configuration (size model.nq)
[in]	v	Joint velocity (size model.nv)
[out]	qout	The integrated configuration (size model.nq)

Definition at line 105 of file joint-configuration.hpp.

integrateCoeffWiseJacobian() [1/2]

void pinocchio::integrateCoeffWiseJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVector > &	q,
		const Eigen::MatrixBase< JacobianMatrix > &	jacobian
	)

Return the Jacobian of the integrate function for the components of the config vector.

Parameters

[in]	model	Model of the kinematic tree.
[in]	q	The joint configuration (vector dim model.nq)
[out]	jacobian	The Jacobian of the integrate operation.

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.

Definition at line 1348 of file joint-configuration.hpp.

integrateCoeffWiseJacobian() [2/2]

void pinocchio::integrateCoeffWiseJacobian	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVector > &	q,
		const Eigen::MatrixBase< JacobianMatrix > &	jacobian
	)

Return the Jacobian of the integrate function for the components of the config vector.

Parameters

[in]	model	Model of the kinematic tree.
[in]	q	The joint configuration (vector dim model.nq)
[out]	jacobian	The Jacobian of the integrate operation.

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.

Definition at line 1348 of file joint-configuration.hpp.

interpolate() [1/4]

ConfigVectorIn1 pinocchio::interpolate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Scalar &	u
	)

Interpolate two configurations for a given model.

Parameters

[in]	model	Model of the kinematic tree on which the interpolation operation is performed.
[in]	q0	Initial configuration vector (size model.nq)
[in]	q1	Final configuration vector (size model.nq)
[in]	u	u in [0;1] position along the interpolation.

Returns

The interpolated configuration (q0 if u = 0, q1 if u = 1)

Definition at line 1520 of file joint-configuration.hpp.

interpolate() [2/4]

ConfigVectorIn1 pinocchio::interpolate	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Scalar &	u
	)

Interpolate two configurations for a given model.

Parameters

[in]	model	Model of the kinematic tree on which the interpolation operation is performed.
[in]	q0	Initial configuration vector (size model.nq)
[in]	q1	Final configuration vector (size model.nq)
[in]	u	u in [0;1] position along the interpolation.

Returns

The interpolated configuration (q0 if u = 0, q1 if u = 1)

Definition at line 1520 of file joint-configuration.hpp.

interpolate() [3/4]

void pinocchio::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 two configurations for a given model.

Parameters

[in]	model	Model of the kinematic tree on which the interpolation is performed.
[in]	q0	Initial configuration vector (size model.nq)
[in]	q1	Final configuration vector (size model.nq)
[in]	u	u in [0;1] position along the interpolation.
[out]	qout	The interpolated configuration (q0 if u = 0, q1 if u = 1)

Definition at line 163 of file joint-configuration.hpp.

interpolate() [4/4]

void pinocchio::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 two configurations for a given model.

Parameters

[in]	model	Model of the kinematic tree on which the interpolation is performed.
[in]	q0	Initial configuration vector (size model.nq)
[in]	q1	Final configuration vector (size model.nq)
[in]	u	u in [0;1] position along the interpolation.
[out]	qout	The interpolated configuration (q0 if u = 0, q1 if u = 1)

Definition at line 163 of file joint-configuration.hpp.

isNormalized() [1/2]

bool pinocchio::isNormalized	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Scalar &	prec = Eigen::NumTraits< Scalar >::dummy_precision()
	)

Check whether a configuration vector is normalized within the given precision provided by prec.

Parameters

[in]	model	Model of the kinematic tree.
[in]	q	Configuration to check (size model.nq).
[in]	prec	Precision.

Returns

Whether the configuration is normalized or not, within the given precision.

Definition at line 1231 of file joint-configuration.hpp.

isNormalized() [2/2]

bool pinocchio::isNormalized	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Scalar &	prec = Eigen::NumTraits<Scalar>::dummy_precision()
	)

Check whether a configuration vector is normalized within the given precision provided by prec.

Parameters

[in]	model	Model of the kinematic tree.
[in]	q	Configuration to check (size model.nq).
[in]	prec	Precision.

Returns

Whether the configuration is normalized or not, within the given precision.

Definition at line 1231 of file joint-configuration.hpp.

isSameConfiguration() [1/2]

bool pinocchio::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 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.).

Parameters

[in]	model	Model of the kinematic tree.
[in]	q1	The first configuraiton to compare.
[in]	q2	The second configuration to compare.
[in]	prec	precision of the comparison.

Returns

Whether the configurations are equivalent or not, 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.).

Parameters

[in]	model	Model of the kinematic tree.
[in]	q1	The first configuraiton to compare
[in]	q2	The second configuration to compare
[in]	prec	precision of the comparison.

Returns

Whether the configurations are equivalent or not

Definition at line 1293 of file joint-configuration.hpp.

isSameConfiguration() [2/2]

bool pinocchio::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 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.).

Parameters

[in]	model	Model of the kinematic tree.
[in]	q1	The first configuraiton to compare
[in]	q2	The second configuration to compare
[in]	prec	precision of the comparison.

Returns

Whether the configurations are equivalent or not

Definition at line 1293 of file joint-configuration.hpp.

jacobianCenterOfMass() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x& pinocchio::jacobianCenterOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const bool	computeSubtreeComs = true
	)

Computes both the jacobian and the the center of mass position of a given model according to the current value stored in data. It assumes that forwardKinematics has been called first. The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (.

See also

pinocchio::computeJointJacobians). And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	computeSubtreeComs	If true, the algorithm also computes the center of mass of the subtrees, expressed in the world coordinate frame.

Returns

The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).

jacobianCenterOfMass() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x& pinocchio::jacobianCenterOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const bool	computeSubtreeComs = true
	)

Computes both the jacobian and the the center of mass position of a given model according to a particular joint configuration. The results are accessible through data.Jcom and data.com[0] and are both expressed in the world frame. In addition, the algorithm also computes the Jacobian of all the joints (.

See also

pinocchio::computeJointJacobians). And data.com[i] gives the center of mass of the subtree supported by joint i (expressed in the world frame).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	computeSubtreeComs	If true, the algorithm also computes the centers of mass of the subtrees, expressed in the world coordinate frame.

Returns

The jacobian of center of mass position of the rigid body system expressed in the world frame (matrix 3 x model.nv).

jacobianSubtreeCenterOfMass() [1/2]

void pinocchio::jacobianSubtreeCenterOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const JointIndex &	rootSubtreeId,
		const Eigen::MatrixBase< Matrix3xLike > &	res
	)

Computes the Jacobian of the center of mass of the given subtree according to a particular joint configuration. In addition, the algorithm also computes the Jacobian of all the joints (.

See also

pinocchio::computeJointJacobians).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
Matrix3xLike	Type of the output Jacobian matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	rootSubtreeId	Index of the parent joint supporting the subtree.
[out]	res	The Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must first fill J with zero elements, e.g. J.setZero().

jacobianSubtreeCenterOfMass() [2/2]

void pinocchio::jacobianSubtreeCenterOfMass	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const JointIndex &	rootSubtreeId,
		const Eigen::MatrixBase< Matrix3xLike > &	res
	)

Computes the Jacobian of the center of mass of the given subtree according to the current value stored in data. It assumes that forwardKinematics has been called first.

Template Parameters

JointCollection	Collection of Joint types.
Matrix3xLike	Type of the output Jacobian matrix.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	rootSubtreeId	Index of the parent joint supporting the subtree.
[out]	res	The Jacobian matrix where the results will be stored in (dim 3 x model.nv). You must first fill J with zero elements, e.g. J.setZero().

jointBodyRegressor()

DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType& pinocchio::jointBodyRegressor ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, JointIndex  joint_id  )

inline

Computes the regressor for the dynamic parameters of a rigid body attached to a given joint, puts the result in data.bodyRegressor and returns it.

This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.

The result is such that \( f = \text{jointBodyRegressor(model,data,joint_id) * I.toDynamicParameters()} \) where \( f \) is the net force acting on the body, including gravity

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	joint_id	The id of the joint.

Returns

The regressor of the body.

lcaba()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::lcaba ( 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 > &  tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &  constraint_models, std::vector< ConstraintData, ConstraintDataAllocator > &  constraint_datas, ProximalSettingsTpl< Scalar > &  settings  )

inline

The closed-loop constrained Articulated Body Algorithm (CLconstrainedABA). It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system for mechanisms potentially with internal closed kinematic loops.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
ConstraintModelAllocator	Allocator class for the std::vector.
ConstraintDataAllocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	constraint_models	Vector of contact models.
[in]	constraint_datas	Vector of contact data.
[in]	settings	Proximal settings (mu, accuracy and maximal number of iterations).

Note

A hint: a typical value of mu in proximal settings is 1e-6, and should always be positive. This also overwrites data.f, possibly leaving it in an inconsistent state.

Returns

A reference to the joint acceleration stored in data.ddq. data.lambdaA[0] stores the constraint forces.

lieGroup() [1/4]

LieGroup_t::template operationProduct<Scalar, Options>::type pinocchio::lieGroup

(

const ModelTpl< Scalar, Options, JointCollectionTpl > &

model

)

Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints.

Parameters

[in]

model

Model of the kinematic tree.

Returns

The cartesian Lie group object.

Definition at line 1824 of file joint-configuration.hpp.

lieGroup() [2/4]

LieGroupMap::template operationProduct<Scalar, Options>::type pinocchio::lieGroup

(

const ModelTpl< Scalar, Options, JointCollectionTpl > &

model

)

Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints.

Parameters

[in]

model

Model of the kinematic tree.

Returns

The cartesian Lie group object.

Definition at line 1824 of file joint-configuration.hpp.

lieGroup() [3/4]

void pinocchio::lieGroup	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		typename LieGroup_t::template operationProduct< Scalar, Options >::type &	lgo
	)

Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints.

Parameters

[in]	model	Model of the kinematic tree.
[out]	lgo	The cartesian Lie group object.

lieGroup() [4/4]

void pinocchio::lieGroup	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		typename LieGroupMap::template operationProduct< Scalar, Options >::type &	lgo
	)

Returns the Lie group associated to the model. It is the cartesian product of the lie groups of all its joints.

Parameters

[in]	model	Model of the kinematic tree.
[out]	lgo	The cartesian Lie group object.

Definition at line 1386 of file joint-configuration.hpp.

neutral() [1/4]

Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> pinocchio::neutral

(

const ModelTpl< Scalar, Options, JointCollectionTpl > &

model

)

Return the neutral configuration element related to the model configuration space.

Parameters

[in]

model

Model of the kinematic tree on which the neutral element is computed.

Returns

The neutral configuration element (size model.nq).

Definition at line 1791 of file joint-configuration.hpp.

neutral() [2/4]

Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> pinocchio::neutral

(

const ModelTpl< Scalar, Options, JointCollectionTpl > &

model

)

Return the neutral configuration element related to the model configuration space.

Parameters

[in]

model

Model of the kinematic tree on which the neutral element is computed.

Returns

The neutral configuration element (size model.nq).

Definition at line 1791 of file joint-configuration.hpp.

neutral() [3/4]

void pinocchio::neutral	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

Return the neutral configuration element related to the model configuration space.

Parameters

[in]	model	Model of the kinematic tree on which the neutral element is computed
[out]	qout	The neutral configuration element (size model.nq).
[in]	model	Model of the kinematic tree on which the neutral element is computed.
[out]	qout	The neutral configuration element (size model.nq).

Definition at line 403 of file joint-configuration.hpp.

neutral() [4/4]

void pinocchio::neutral	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

Return the neutral configuration element related to the model configuration space.

Parameters

[in]	model	Model of the kinematic tree on which the neutral element is computed.
[out]	qout	The neutral configuration element (size model.nq).

Definition at line 403 of file joint-configuration.hpp.

nonLinearEffects()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::nonLinearEffects	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentVectorType > &	v
	)

Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the bias terms \( b(q,\dot{q}) \) of the Lagrangian dynamics:

\( \begin{eqnarray} M \ddot{q} + b(q, \dot{q}) = \tau \end{eqnarray} \)

Note

This function is equivalent to pinocchio::rnea(model, data, q, v, 0).

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType	Type of the joint velocity vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).

Returns

The bias terms stored in data.nle.

normalize() [1/2]

void pinocchio::normalize	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	qout
	)

Normalize a configuration vector.

Parameters

[in]	model	Model of the kinematic tree.
[in,out]	qout	Configuration to normalize (size model.nq).

Definition at line 1184 of file joint-configuration.hpp.

normalize() [2/2]

void pinocchio::normalize	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	qout
	)

Normalize a configuration vector.

Parameters

[in]	model	Model of the kinematic tree.
[in,out]	qout	Configuration to normalize (size model.nq).

Definition at line 1184 of file joint-configuration.hpp.

PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS() [1/3]

pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS

(

(typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)

)

const

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

Parameters

[in]	model	Model of the kinematic tree on which the uniform sampling operation is performed.
[in]	lowerLimits	Joints lower limits (size model.nq).
[in]	upperLimits	Joints upper limits (size model.nq).

Returns

The resulting configuration vector (size model.nq).

PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS() [2/3]

PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS

(

(typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)

)

const

Generate a configuration vector uniformly sampled among provided limits.

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, exceptions may be thrown in the joint implementation of uniformlySample

Parameters

[in]	model	Model of the kinematic tree on which the uniform sampling operation is performed.
[in]	lowerLimits	Joints lower limits (size model.nq).
[in]	upperLimits	Joints upper limits (size model.nq).

Returns

The resulting configuration vector (size model.nq)

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

Parameters

[in]

model

Model of the kinematic tree on which the uniform sampling operation is performed.

Returns

The resulting configuration vector (size model.nq)

PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS() [3/3]

pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE_NO_PARENS

(

(typename ModelTpl< Scalar, Options, JointCollectionTpl >::ConfigVectorType)

)

const

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

Parameters

[in]

model

Model of the kinematic tree on which the uniform sampling operation is performed.

Returns

The resulting configuration vector (size model.nq).

pv()

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::pv ( 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 > &  tau, const std::vector< ConstraintModel, ConstraintModelAllocator > &  contact_models, std::vector< ConstraintData, ConstraintDataAllocator > &  contact_datas, ProximalSettingsTpl< Scalar > &  settings  )

inline

The Popov-Vereshchagin algorithm. It computes constrained forward dynamics, aka the joint accelerations and constraint forces given the current state, actuation and the constraints on the system. All the quantities are expressed in the LOCAL coordinate systems of the joint frames.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint torque vector.
Allocator	Allocator class for the std::vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	tau	The joint torque vector (dim model.nv).
[in]	contact_models	Vector of contact models.
[in]	contact_datas	Vector of contact data.
[in]	settings	Proximal settings (mu, accuracy and maximal number of iterations).

Note

This also overwrites data.f, possibly leaving it in an inconsistent state.

Returns

A reference to the joint acceleration stored in data.ddq. data.lambdaA[0] stores the constraint forces.

randomConfiguration() [1/2]

void pinocchio::randomConfiguration	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	lowerLimits,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	upperLimits,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

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.

Parameters

[in]	model	Model of the system on which the random configuration operation is performed.
[in]	lowerLimits	Joints lower limits (size model.nq).
[in]	upperLimits	Joints upper limits (size model.nq).
[out]	qout	The resulting configuration vector (size model.nq).

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

Parameters

[in]	model	Model of the system on which the random configuration operation is performed.
[in]	lowerLimits	Joints lower limits (size model.nq).
[in]	upperLimits	Joints upper limits (size model.nq).
[out]	qout	The resulting configuration vector (size model.nq).

Definition at line 354 of file joint-configuration.hpp.

randomConfiguration() [2/2]

void pinocchio::randomConfiguration	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	lowerLimits,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	upperLimits,
		const Eigen::MatrixBase< ReturnType > &	qout
	)

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

Parameters

[in]	model	Model of the system on which the random configuration operation is performed.
[in]	lowerLimits	Joints lower limits (size model.nq).
[in]	upperLimits	Joints upper limits (size model.nq).
[out]	qout	The resulting configuration vector (size model.nq).

Definition at line 354 of file joint-configuration.hpp.

reachableWorkspace()

void pinocchio::reachableWorkspace	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q0,
		const double	time_horizon,
		const int	frame_id,
		Eigen::MatrixXd &	vertex,
		const ReachableSetParams &	params = ReachableSetParams()
	)

Computes the reachable workspace on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	q0	The initial joint configuration vector (dim model.nq).
[in]	time_horizon	time horizon for which the polytope will be computed (in seconds)
[in]	frame_id	Index of the frame for which the workspace should be computed.
[in]	params	parameters of the algorithm
[out]	vertex	inside of the reachable workspace

reachableWorkspaceHull()

void pinocchio::reachableWorkspaceHull	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q0,
		const double	time_horizon,
		const int	frame_id,
		ReachableSetResults &	res,
		const ReachableSetParams &	params = ReachableSetParams()
	)

Computes the convex Hull reachable workspace on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	q0	The initial joint configuration vector (dim model.nq).
[in]	frame_id	Index of the frame for which the workspace should be computed.
[in]	time_horizon	time horizon for which the polytope will be computed (in seconds)
[in]	params	parameters of the algorithm
[out]	res	Results of algorithm

reachableWorkspaceWithCollisions()

void pinocchio::reachableWorkspaceWithCollisions	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const GeometryModel &	geom_model,
		const Eigen::MatrixBase< ConfigVectorType > &	q0,
		const double	time_horizon,
		const int	frame_id,
		Eigen::MatrixXd &	vertex,
		const ReachableSetParams &	params = ReachableSetParams()
	)

Computes the reachable workspace with respect to a geometry model on a fixed time horizon. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	geom_model	Geometry model, to take into accout collision with the environment
[in]	q0	The initial joint configuration vector (dim model.nq).
[in]	frame_id	Index of the frame for which the workspace should be computed.
[in]	time_horizon	time horizon for which the polytope will be computed (in seconds)
[in]	params	parameters of the algorithm
[out]	vertex	inside of the reachable workspace

reachableWorkspaceWithCollisionsHull()

void pinocchio::reachableWorkspaceWithCollisionsHull	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const GeometryModel &	geom_model,
		const Eigen::MatrixBase< ConfigVectorType > &	q0,
		const double	time_horizon,
		const int	frame_id,
		ReachableSetResults &	res,
		const ReachableSetParams &	params = ReachableSetParams()
	)

Computes the convex Hull of the reachable workspace with respect to a geometry model on a fixed time horizon. Make sure that reachable workspace takes into account collisions with environment. For more information, please see https://gitlab.inria.fr/auctus-team/people/antunskuric/pycapacity.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	geom_model	Geometry model, to take into accout collision with the environment
[in]	q0	The initial joint configuration vector (dim model.nq).
[in]	frame_id	Index of the frame for which the workspace should be computed.
[in]	time_horizon	time horizon for which the polytope will be computed (in seconds)
[in]	params	parameters of the algorithm
[out]	res	Results of algorithm

rnea() [1/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::rnea	(	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
	)

The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).

Returns

The desired joint torques stored in data.tau.

rnea() [2/2]

const DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType& pinocchio::rnea	(	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,
		const std::vector< ForceDerived > &	fext
	)

The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system, the desired joint accelerations and the external forces.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.
TangentVectorType1	Type of the joint velocity vector.
TangentVectorType2	Type of the joint acceleration vector.
ForceDerived	Type of the external forces.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	q	The joint configuration vector (dim model.nq).
[in]	v	The joint velocity vector (dim model.nv).
[in]	a	The joint acceleration vector (dim model.nv).
[in]	fext	Vector of external forces expressed in the local frame of the joints (dim model.njoints)

Returns

The desired joint torques stored in data.tau.

rneaInParallel()

void pinocchio::rneaInParallel	(	const size_t	num_threads,
		ModelPoolTpl< Scalar, Options, JointCollectionTpl > &	pool,
		const Eigen::MatrixBase< ConfigVectorPool > &	q,
		const Eigen::MatrixBase< TangentVectorPool1 > &	v,
		const Eigen::MatrixBase< TangentVectorPool2 > &	a,
		const Eigen::MatrixBase< TangentVectorPool3 > &	tau
	)

The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques according to the current state of the system and the desired joint accelerations.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorPool	Matrix type of the joint configuration vector.
TangentVectorPool1	Matrix type of the joint velocity vector.
TangentVectorPool2	Matrix type of the joint acceleration vector.
TangentVectorPool3	Matrix type of the joint torque vector.

Parameters

[in]	pool	Pool containing model and data for parallel computations.
[in]	num_threads	Number of threads used for parallel computations.
[in]	q	The joint configuration vector (dim model.nq x batch_size).
[in]	v	The joint velocity vector (dim model.nv x batch_size).
[in]	a	The joint acceleration vector (dim model.nv x batch_size).
[out]	tau	The joint torque vector (dim model.nv x batch_size).

squaredDistance() [1/4]

ConfigVectorIn1 pinocchio::squaredDistance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Squared distance between two configurations.

Parameters

[in]	model	Model of the kinematic tree on which the squared distance operation is performed.
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints)

Squared distance between two configurations.

Parameters

[in]	model	Model of the kinematic tree on which the squared distance operation is performed.
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints)

Definition at line 1629 of file joint-configuration.hpp.

squaredDistance() [2/4]

ConfigVectorIn1 pinocchio::squaredDistance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Squared distance between two configuration vectors.

Squared distance between two configurations.

Parameters

[in]	model	Model of the kinematic tree on which the squared distance operation is performed.
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The corresponding squared distances for each joint (size model.njoints-1, corresponding to the number of joints)

Definition at line 1629 of file joint-configuration.hpp.

squaredDistance() [3/4]

void pinocchio::squaredDistance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Eigen::MatrixBase< ReturnType > &	out
	)

Squared distance between two configuration vectors.

Parameters

[in]	model	Model of the system on which the squared distance operation is performed.
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)
[out]	out	The corresponding squared distances for each joint (size model.njoints-1 = number of joints).

Definition at line 287 of file joint-configuration.hpp.

squaredDistance() [4/4]

void pinocchio::squaredDistance	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1,
		const Eigen::MatrixBase< ReturnType > &	out
	)

Squared distance between two configuration vectors.

Parameters

[in]	model	Model of the system on which the squared distance operation is performed.
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)
[out]	out	The corresponding squared distances for each joint (size model.njoints-1 = number of joints).

Definition at line 287 of file joint-configuration.hpp.

squaredDistanceSum() [1/2]

Scalar pinocchio::squaredDistanceSum	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Overall squared distance between two configuration vectors.

Parameters

[in]	model	Model we want to compute the distance
[in]	q0	Configuration 0 (size model.nq)
[in]	q1	Configuration 1 (size model.nq)

Returns

The squared distance between the two configurations

Overall squared distance between two configuration vectors.

Parameters

[in]	model	Model of the kinematic tree
[in]	q0	Configuration from (size model.nq)
[in]	q1	Configuration to (size model.nq)

Returns

The squared distance between the two configurations q0 and q1.

Definition at line 1090 of file joint-configuration.hpp.

squaredDistanceSum() [2/2]

Scalar pinocchio::squaredDistanceSum	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorIn1 > &	q0,
		const Eigen::MatrixBase< ConfigVectorIn2 > &	q1
	)

Overall squared distance between two configuration vectors, namely \( || q_{1} \ominus q_{0} ||_2^{2} \).

Overall squared distance between two configuration vectors.

Parameters

[in]	model	Model of the kinematic tree
[in]	q0	Configuration from (size model.nq)
[in]	q1	Configuration to (size model.nq)

Returns

The squared distance between the two configurations q0 and q1.

Definition at line 1090 of file joint-configuration.hpp.

tangentMap() [1/2]

void pinocchio::tangentMap	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentMapMatrixType > &	TM,
		const AssignmentOperatorType	op = SETTO
	)

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).

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[out]	TM	Tangent map in q mapping the Lie algebra with the parametric tangent space.
[in]	op	Operation to apply.

Definition at line 600 of file joint-configuration.hpp.

tangentMap() [2/2]

void pinocchio::tangentMap	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		const Eigen::MatrixBase< ConfigVectorType > &	q,
		const Eigen::MatrixBase< TangentMapMatrixType > &	TM,
		const AssignmentOperatorType	op = SETTO
	)

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).

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[out]	TM	Tangent map in q mapping the Lie algebra with the parametric tangent space.
[in]	op	Operation to apply.

Definition at line 600 of file joint-configuration.hpp.

tangentMapProduct() [1/2]

void pinocchio::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
	)

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	mat_in	A matrix with range in the Lie algebra (size model.nv, n_cols)
[out]	mat_out	A matrix with range in the parametric space (size model.nq, n_cols)
[in]	op	Operation to apply.

Definition at line 716 of file joint-configuration.hpp.

tangentMapProduct() [2/2]

void pinocchio::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
	)

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	mat_in	A matrix with range in the Lie algebra (size model.nv, n_cols)
[out]	mat_out	A matrix with range in the parametric space (size model.nq, n_cols)
[in]	op	Operation to apply.

Definition at line 716 of file joint-configuration.hpp.

tangentMapTransposeProduct() [1/2]

void pinocchio::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
	)

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	mat_in	A matrix with range in the parametric space (size model.nq, n_cols)
[out]	mat_out	A matrix with range in the Lie algebra (size model.nv, n_cols)
[in]	op	Operation to apply.

Definition at line 780 of file joint-configuration.hpp.

tangentMapTransposeProduct() [2/2]

void pinocchio::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
	)

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.

Parameters

[in]	model	Model of the kinematic tree on which the integration operation is performed.
[in]	q	Initial configuration (size model.nq)
[in]	mat_in	A matrix with range in the parametric space (size model.nq, n_cols)
[out]	mat_out	A matrix with range in the Lie algebra (size model.nv, n_cols)
[in]	op	Operation to apply.

Definition at line 780 of file joint-configuration.hpp.

transformJointIntoMimic()

void pinocchio::transformJointIntoMimic	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	input_model,
		const JointIndex &	index_mimicked,
		const JointIndex &	index_mimicking,
		const Scalar &	scaling,
		const Scalar &	offset,
		ModelTpl< Scalar, Options, JointCollectionTpl > &	output_model
	)

Transform of a joint of the model into a mimic joint. Keep the type of the joint as it was previously.

Parameters

[in]	input_model	the input model to take joints from.
[in]	index_mimicked	index of the joint to mimic
[in]	index_mimicking	index of the joint that will mimic
[in]	scaling	Scaling of joint velocity and configuration
[in]	offset	Offset of joint configuration
[out]	output_model	Model with the joint mimic

updateFramePlacement()

const DataTpl<Scalar, Options, JointCollectionTpl>::SE3& pinocchio::updateFramePlacement ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const FrameIndex  frame_id  )

inline

Updates the placement of the given frame.

Parameters

[in]	model	The kinematic model.
	data	Data associated to model.
[in]	frame_id	Id of the operational Frame.

Returns

A reference to the frame placement stored in data.oMf[frame_id]

Warning

One of the algorithms forwardKinematics should have been called first

updateFramePlacements()

void pinocchio::updateFramePlacements ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data  )

inline

Updates the position of each frame contained in the model.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The kinematic model.
	data	Data associated to model.

Warning

One of the algorithms forwardKinematics should have been called first.

updateGeometryPlacements() [1/2]

void pinocchio::updateGeometryPlacements ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, const DataTpl< Scalar, Options, JointCollectionTpl > &  data, const GeometryModel &  geom_model, GeometryData &  geom_data  )

inline

Update the placement of the geometry objects according to the current joint placements contained in data.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	geom_model	The geometry model containing the collision objects.
[out]	geom_data	The geometry data containing the placements of the collision objects. See oMg field in GeometryData.

updateGeometryPlacements() [2/2]

void pinocchio::updateGeometryPlacements ( const ModelTpl< Scalar, Options, JointCollectionTpl > &  model, DataTpl< Scalar, Options, JointCollectionTpl > &  data, const GeometryModel &  geom_model, GeometryData &  geom_data, const Eigen::MatrixBase< ConfigVectorType > &  q  )

inline

Apply a forward kinematics and update the placement of the geometry objects.

Template Parameters

JointCollection	Collection of Joint types.
ConfigVectorType	Type of the joint configuration vector.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.
[in]	geom_model	The geometry model containing the collision objects.
[out]	geom_data	The geometry data containing the placements of the collision objects. See oMg field in GeometryData.
[in]	q	The joint configuration vector (dim model.nq).

updateGlobalPlacements()

void pinocchio::updateGlobalPlacements	(	const ModelTpl< Scalar, Options, JointCollectionTpl > &	model,
		DataTpl< Scalar, Options, JointCollectionTpl > &	data
	)

Update the global placement of the joints oMi according to the relative placements of the joints.

Template Parameters

JointCollection

Collection of Joint types.

Parameters

[in]	model	The model structure of the rigid body system.
[in]	data	The data structure of the rigid body system.

Remarks

This algorithm may be useful to call to update global joint placement after calling pinocchio::rnea, pinocchio::aba, etc for example.

Variable Documentation

model

JointCollectionTpl & model

Initial value:

{
    return randomConfiguration<LieGroupMap, Scalar, Options, JointCollectionTpl>(model)

Definition at line 1668 of file joint-configuration.hpp.

upperLimits

JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & upperLimits

Initial value:

{
    return randomConfiguration<
      LieGroupMap, Scalar, Options, JointCollectionTpl, ConfigVectorIn1, ConfigVectorIn2>(
      model, lowerLimits.derived(), upperLimits.derived())

Definition at line 1670 of file joint-configuration.hpp.