expose-contact-dynamics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS, INRIA
3 //
4 
6 #include "pinocchio/algorithm/contact-dynamics.hpp"
7 
8 namespace pinocchio
9 {
10  namespace python
11  {
12 
14  Data & data,
15  const Eigen::VectorXd & q,
16  const Eigen::VectorXd & v,
17  const Eigen::VectorXd & tau,
18  const Eigen::MatrixXd & J,
19  const Eigen::VectorXd & gamma,
20  const double inv_damping = 0.0)
21  {
22  return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
23  }
24 
25  BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8)
26 
28  Data & data,
29  const Eigen::VectorXd & tau,
30  const Eigen::MatrixXd & J,
31  const Eigen::VectorXd & gamma,
32  const double inv_damping = 0.0)
33  {
34  return forwardDynamics(model, data, tau, J, gamma, inv_damping);
35  }
36 
37  BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6)
38 
40  Data & data,
41  const Eigen::VectorXd & q,
42  const Eigen::VectorXd & v_before,
43  const Eigen::MatrixXd & J,
44  const double r_coeff = 0.,
45  const double inv_damping = 0.)
46  {
47  return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
48  }
49 
50  BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7)
51 
53  Data & data,
54  const Eigen::VectorXd & v_before,
55  const Eigen::MatrixXd & J,
56  const double r_coeff = 0.,
57  const double inv_damping = 0.)
58  {
59  return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
60  }
61 
62  BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)
63 
65  Data & data,
66  const Eigen::VectorXd & q,
67  const Eigen::MatrixXd & J,
68  const double mu = 0)
69  {
70  Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
71  computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv, mu);
72  return KKTMatrix_inv;
73  }
74 
75  BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload,
77 
78  static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
79  Data & data,
80  const Eigen::MatrixXd & J)
81  {
82  Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
83  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
84  return KKTMatrix_inv;
85  }
86 
88  {
89  using namespace Eigen;
90 
91  bp::def("forwardDynamics",
93  forwardDynamics_overloads(
94  bp::args("Model","Data","q","v","tau","J","gamma","damping"),
95  "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
96  " Internally, pinocchio.computeAllTerms is called."
97  ));
98 
99  bp::def("forwardDynamics",
101  forwardDynamics_overloads_no_q(
102  bp::args("Model","Data","tau","J","gamma","damping"),
103  "Solves the forward dynamics problem with contacts, puts the result in Data::ddq and return it. The contact forces are stored in data.lambda_c."
104  " Assumes pinocchio.computeAllTerms has been called."
105  ));
106 
107  bp::def("impulseDynamics",
109  impulseDynamics_overloads(
110  bp::args("Model","Data","q","v_before","J","r_coeff","damping"),
111  "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
112  " Internally, pinocchio.crba is called."
113  ));
114 
115  bp::def("impulseDynamics",
117  impulseDynamics_overloads_no_q(
118  bp::args("Model","Data","v_before","J","r_coeff","damping"),
119  "Solves the impact dynamics problem with contacts, store the result in Data::dq_after and return it. The contact impulses are stored in data.impulse_c."
120  " Assumes pinocchio.crba has been called."
121  ));
122 
123  bp::def("computeKKTContactDynamicMatrixInverse",
125  computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","J","damping"),
126  "Computes the inverse of the constraint matrix [[M J^T], [J 0]]."));
127 
128  bp::def("getKKTContactDynamicMatrixInverse",
129  getKKTContactDynamicMatrixInverse_proxy,
130  bp::args("Model","Data","J"),
131  "Computes the inverse of the constraint matrix [[M JT], [J 0]]. forward/impulseDynamics must be called first. The jacobian should be the same that was provided to forward/impulseDynamics.");
132  }
133 
134  } // namespace python
135 } // namespace pinocchio
static const Eigen::VectorXd impulseDynamics_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v_before, const Eigen::MatrixXd &J, const double r_coeff=0., const double inv_damping=0.)
static const Eigen::VectorXd forwardDynamics_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau, const Eigen::MatrixXd &J, const Eigen::VectorXd &gamma, const double inv_damping=0.0)
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...
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::MatrixXd &J, const double mu=0)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model &model, Data &data, const Eigen::VectorXd &v_before, const Eigen::MatrixXd &J, const double r_coeff=0., const double inv_damping=0.)
static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model &model, Data &data, const Eigen::VectorXd &tau, const Eigen::MatrixXd &J, const Eigen::VectorXd &gamma, const double inv_damping=0.0)
Main pinocchio namespace.
Definition: timings.cpp:28
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...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
JointCollectionTpl & model


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29