Namespaces
joint-configuration.hpp File Reference
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/algorithm/joint-configuration.hxx"
Include dependency graph for joint-configuration.hpp:

Go to the source code of this file.

Namespaces

 pinocchio
 Main pinocchio namespace.
 

Functions

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 >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename ReturnType >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 , typename ReturnType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ReturnType >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ReturnType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename JacobianMatrixType >
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=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 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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename JacobianMatrixType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType , typename JacobianMatrixType1 , typename JacobianMatrixType2 >
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 originate 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 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 originate 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 >
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 originate 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 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 originate 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 >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVector1 , typename ConfigVector2 , typename JacobianMatrix >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
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} $. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
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 $. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
Scalar pinocchio::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 >
void pinocchio::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 pinocchio::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 >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorIn1 , typename ConfigVectorIn2 >
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. More...
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVector , typename JacobianMatrix >
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVector , typename JacobianMatrix >
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. More...
 

API that allocates memory

 pinocchio::Options
 
JointCollectionTpl & pinocchio::model
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & pinocchio::q
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & pinocchio::v
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & pinocchio::q0
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & pinocchio::q1
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & pinocchio::u
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & pinocchio::lowerLimits
 
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & pinocchio::upperLimits
 
template<typename LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl, typename ConfigVectorType , typename TangentVectorType >
 pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE (ConfigVectorType) integrate(const ModelTpl< Scalar
 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 >
 pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE (ConfigVectorType) integrate(const ModelTpl< Scalar
 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 ConfigVectorIn1 , typename ConfigVectorIn2 >
 pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE (ConfigVectorIn1) interpolate(const ModelTpl< Scalar
 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 >
 pinocchio::PINOCCHIO_EIGEN_PLAIN_TYPE (ConfigVectorIn1) difference(const ModelTpl< Scalar
 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 >
 pinocchio::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::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 LieGroup_t , typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
 pinocchio::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 Scalar , int Options, template< typename, int > class JointCollectionTpl>
 pinocchio::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>
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. More...
 
template<typename Scalar , int Options, template< typename, int > class JointCollectionTpl>
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. More...
 


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:05