pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
contact-dynamics.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_contact_dynamics_hpp__
6 #define __pinocchio_contact_dynamics_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 namespace pinocchio
12 {
13 
29 
43  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
44  typename ConstraintMatrixType, typename DriftVectorType>
45  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
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.);
54 
70 
83  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename TangentVectorType,
84  typename ConstraintMatrixType, typename DriftVectorType>
85  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
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.);
92 
112 
127  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
128  typename ConstraintMatrixType, typename DriftVectorType>
129  PINOCCHIO_DEPRECATED
130  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
131  forwardDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
132  DataTpl<Scalar,Options,JointCollectionTpl> & data,
133  const Eigen::MatrixBase<ConfigVectorType> & q,
134  const Eigen::MatrixBase<TangentVectorType1> & v,
135  const Eigen::MatrixBase<TangentVectorType2> & tau,
136  const Eigen::MatrixBase<ConstraintMatrixType> & J,
137  const Eigen::MatrixBase<DriftVectorType> & gamma,
138  const Scalar inv_damping,
139  const bool updateKinematics)
140  {
141  if(updateKinematics)
142  return forwardDynamics(model,data,q,v,tau,J,gamma,inv_damping);
143  else
144  return forwardDynamics(model,data,tau,J,gamma,inv_damping);
145  }
146 
147 
158  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
159  typename ConstraintMatrixType, typename KKTMatrixType>
160  inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
161  const DataTpl<Scalar,Options,JointCollectionTpl> & data,
162  const Eigen::MatrixBase<ConstraintMatrixType> & J,
163  const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv);
164 
188  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
189  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
190  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
191  DataTpl<Scalar,Options,JointCollectionTpl> & data,
192  const Eigen::MatrixBase<ConfigVectorType> & q,
193  const Eigen::MatrixBase<TangentVectorType> & v_before,
194  const Eigen::MatrixBase<ConstraintMatrixType> & J,
195  const Scalar r_coeff = 0.,
196  const Scalar inv_damping = 0.);
197 
220  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
221  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
222  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
223  DataTpl<Scalar,Options,JointCollectionTpl> & data,
224  const Eigen::MatrixBase<TangentVectorType> & v_before,
225  const Eigen::MatrixBase<ConstraintMatrixType> & J,
226  const Scalar r_coeff = 0.,
227  const Scalar inv_damping = 0.);
228 
256  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename ConstraintMatrixType>
257  PINOCCHIO_DEPRECATED
258  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
259  impulseDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
260  DataTpl<Scalar,Options,JointCollectionTpl> & data,
261  const Eigen::MatrixBase<ConfigVectorType> & q,
262  const Eigen::MatrixBase<TangentVectorType> & v_before,
263  const Eigen::MatrixBase<ConstraintMatrixType> & J,
264  const Scalar r_coeff,
265  const bool updateKinematics)
266  {
267  if(updateKinematics)
268  return impulseDynamics(model,data,q,v_before,J,r_coeff,Scalar(0));
269  else
270  return impulseDynamics(model,data,v_before,J,r_coeff,Scalar(0));
271  }
272 
273 } // namespace pinocchio
274 
275 #include "pinocchio/algorithm/contact-dynamics.hxx"
276 
277 #endif // ifndef __pinocchio_contact_dynamics_hpp__
pinocchio::getKKTContactDynamicMatrixInverse
void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &MJtJ_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints, [[M JT]...
pinocchio::impulseDynamics
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...
pinocchio::DataTpl::TangentVectorType
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).
Definition: data.hpp:68
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::forwardDynamics
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...