5 #ifndef __pinocchio_contact_dynamics_hpp__ 6 #define __pinocchio_contact_dynamics_hpp__ 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/multibody/data.hpp" 43 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2,
44 typename ConstraintMatrixType,
typename DriftVectorType>
46 forwardDynamics(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
47 DataTpl<Scalar,Options,JointCollectionTpl> & data,
48 const Eigen::MatrixBase<ConfigVectorType> & q,
49 const Eigen::MatrixBase<TangentVectorType1> & v,
50 const Eigen::MatrixBase<TangentVectorType2> & tau,
51 const Eigen::MatrixBase<ConstraintMatrixType> & J,
52 const Eigen::MatrixBase<DriftVectorType> & gamma,
53 const Scalar inv_damping = 0.);
83 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename TangentVectorType,
84 typename ConstraintMatrixType,
typename DriftVectorType>
86 forwardDynamics(
const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
87 DataTpl<Scalar,Options,JointCollectionTpl> & data,
88 const Eigen::MatrixBase<TangentVectorType> & tau,
89 const Eigen::MatrixBase<ConstraintMatrixType> & J,
90 const Eigen::MatrixBase<DriftVectorType> & gamma,
91 const Scalar inv_damping = 0.);
126 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType1,
typename TangentVectorType2,
127 typename ConstraintMatrixType,
typename DriftVectorType>
132 const Eigen::MatrixBase<ConfigVectorType> & q,
133 const Eigen::MatrixBase<TangentVectorType1> & v,
134 const Eigen::MatrixBase<TangentVectorType2> & tau,
135 const Eigen::MatrixBase<ConstraintMatrixType> & J,
136 const Eigen::MatrixBase<DriftVectorType> & gamma,
137 const Scalar inv_damping,
138 const bool updateKinematics)
157 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
158 typename ConfigVectorType,
typename ConstraintMatrixType,
typename KKTMatrixType>
161 const Eigen::MatrixBase<ConfigVectorType> & q,
162 const Eigen::MatrixBase<ConstraintMatrixType> & J,
163 const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
164 const Scalar & inv_damping = 0.);
181 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
182 typename ConstraintMatrixType,
typename KKTMatrixType>
185 const Eigen::MatrixBase<ConstraintMatrixType> & J,
186 const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv);
211 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ConstraintMatrixType>
215 const Eigen::MatrixBase<ConfigVectorType> & q,
216 const Eigen::MatrixBase<TangentVectorType> & v_before,
217 const Eigen::MatrixBase<ConstraintMatrixType> & J,
218 const Scalar r_coeff = 0.,
219 const Scalar inv_damping = 0.);
243 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ConstraintMatrixType>
247 const Eigen::MatrixBase<TangentVectorType> & v_before,
248 const Eigen::MatrixBase<ConstraintMatrixType> & J,
249 const Scalar r_coeff = 0.,
250 const Scalar inv_damping = 0.);
279 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorType,
typename TangentVectorType,
typename ConstraintMatrixType>
284 const Eigen::MatrixBase<ConfigVectorType> & q,
285 const Eigen::MatrixBase<TangentVectorType> & v_before,
286 const Eigen::MatrixBase<ConstraintMatrixType> & J,
287 const Scalar r_coeff,
288 const bool updateKinematics)
298 #include "pinocchio/algorithm/contact-dynamics.hxx" 300 #endif // ifndef __pinocchio_contact_dynamics_hpp__ 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. It computes the followi...
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...
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 followi...
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).
Main pinocchio namespace.
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 call...