Program Listing for File contact-dynamics.hpp
↰ Return to documentation for file (include/pinocchio/algorithm/contact-dynamics.hpp
)
//
// Copyright (c) 2016-2021 CNRS INRIA
//
#ifndef __pinocchio_contact_dynamics_hpp__
#define __pinocchio_contact_dynamics_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
namespace pinocchio
{
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename 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.);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
typename ConstraintMatrixType, typename DriftVectorType>
inline const typename 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.);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename ConstraintMatrixType, typename DriftVectorType>
PINOCCHIO_DEPRECATED
inline const typename 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,
const bool updateKinematics)
{
if(updateKinematics)
return forwardDynamics(model,data,q,v,tau,J,gamma,inv_damping);
else
return forwardDynamics(model,data,tau,J,gamma,inv_damping);
}
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.);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConstraintMatrixType, typename KKTMatrixType>
inline 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);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename 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.);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
inline const typename 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.);
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
PINOCCHIO_DEPRECATED
inline const typename 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,
const bool updateKinematics)
{
if(updateKinematics)
return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
else
return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
}
} // namespace pinocchio
#include "pinocchio/algorithm/contact-dynamics.hxx"
#endif // ifndef __pinocchio_contact_dynamics_hpp__