pinocchio 4.1.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
contact-dynamics.hpp
1//
2// Copyright (c) 2016-2020 CNRS INRIA
3//
4#pragma once
5
6// IWYU pragma: begin_keep
7#include <cassert>
8#include <cstddef>
9#include <cmath>
10#include <complex>
11
12#include <Eigen/Core>
13#include <Eigen/Cholesky>
14
15#include "pinocchio/macros.hpp"
16#include "pinocchio/unsupported.hpp"
17#include "pinocchio/eigen-common.hpp"
18
19#include "pinocchio/math.hpp"
20#include "pinocchio/multibody.hpp"
21
22#include "pinocchio/algorithm/cholesky.hpp"
23#include "pinocchio/algorithm/check.hpp"
24#include "pinocchio/algorithm/crba.hpp"
25#include "pinocchio/algorithm/compute-all-terms.hpp"
26// IWYU pragma: end_keep
27
28namespace pinocchio
29{
30
56
73 template<
74 typename Scalar,
75 int Options,
76 template<typename, int> class JointCollectionTpl,
77 typename ConfigVectorType,
78 typename TangentVectorType1,
79 typename TangentVectorType2,
80 typename ConstraintMatrixType,
81 typename DriftVectorType>
82 PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
83 TangentVectorType &
84 forwardDynamics(
85 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86 DataTpl<Scalar, Options, JointCollectionTpl> & data,
87 const Eigen::MatrixBase<ConfigVectorType> & q,
88 const Eigen::MatrixBase<TangentVectorType1> & v,
89 const Eigen::MatrixBase<TangentVectorType2> & tau,
90 const Eigen::MatrixBase<ConstraintMatrixType> & J,
91 const Eigen::MatrixBase<DriftVectorType> & gamma,
92 const Scalar inv_damping = 0.);
93
119
134 template<
135 typename Scalar,
136 int Options,
137 template<typename, int> class JointCollectionTpl,
138 typename TangentVectorType,
139 typename ConstraintMatrixType,
140 typename DriftVectorType>
141 PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
142 TangentVectorType &
143 forwardDynamics(
144 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
145 DataTpl<Scalar, Options, JointCollectionTpl> & data,
146 const Eigen::MatrixBase<TangentVectorType> & tau,
147 const Eigen::MatrixBase<ConstraintMatrixType> & J,
148 const Eigen::MatrixBase<DriftVectorType> & gamma,
149 const Scalar inv_damping = 0.);
150
167 template<
168 typename Scalar,
169 int Options,
170 template<typename, int> class JointCollectionTpl,
171 typename ConfigVectorType,
172 typename ConstraintMatrixType,
173 typename KKTMatrixType>
174 void computeKKTContactDynamicMatrixInverse(
175 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
176 DataTpl<Scalar, Options, JointCollectionTpl> & data,
177 const Eigen::MatrixBase<ConfigVectorType> & q,
178 const Eigen::MatrixBase<ConstraintMatrixType> & J,
179 const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
180 const Scalar & inv_damping = 0.);
181
204 template<
205 typename Scalar,
206 int Options,
207 template<typename, int> class JointCollectionTpl,
208 typename ConstraintMatrixType,
209 typename KKTMatrixType>
210 PINOCCHIO_UNSUPPORTED void getKKTContactDynamicMatrixInverse(
211 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
212 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
213 const Eigen::MatrixBase<ConstraintMatrixType> & J,
214 const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv);
215
251 template<
252 typename Scalar,
253 int Options,
254 template<typename, int> class JointCollectionTpl,
255 typename ConfigVectorType,
256 typename TangentVectorType,
257 typename ConstraintMatrixType>
258 PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
259 TangentVectorType &
260 impulseDynamics(
261 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
262 DataTpl<Scalar, Options, JointCollectionTpl> & data,
263 const Eigen::MatrixBase<ConfigVectorType> & q,
264 const Eigen::MatrixBase<TangentVectorType> & v_before,
265 const Eigen::MatrixBase<ConstraintMatrixType> & J,
266 const Scalar r_coeff = 0.,
267 const Scalar inv_damping = 0.);
268
303 template<
304 typename Scalar,
305 int Options,
306 template<typename, int> class JointCollectionTpl,
307 typename ConfigVectorType,
308 typename TangentVectorType,
309 typename ConstraintMatrixType>
310 PINOCCHIO_UNSUPPORTED inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::
311 TangentVectorType &
312 impulseDynamics(
313 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
314 DataTpl<Scalar, Options, JointCollectionTpl> & data,
315 const Eigen::MatrixBase<TangentVectorType> & v_before,
316 const Eigen::MatrixBase<ConstraintMatrixType> & J,
317 const Scalar r_coeff = 0.,
318 const Scalar inv_damping = 0.);
319
320} // namespace pinocchio
321
322// IWYU pragma: begin_exports
323#include "pinocchio/src/algorithm/contact-dynamics.hxx"
324// IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::forwardDynamics
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 call...
pinocchio::computeKKTContactDynamicMatrixInverse
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...
pinocchio::impulseDynamics
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.
pinocchio::getKKTContactDynamicMatrixInverse
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.