expose-contact-dynamics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020-2021 INRIA
3 //
4 
7 
8 namespace pinocchio
9 {
10  namespace python
11  {
12 
14  const context::Model & model,
16  const context::VectorXs & q,
17  const context::VectorXs & v,
18  const context::VectorXs & tau,
19  const context::MatrixXs & J,
20  const context::VectorXs & gamma,
22  {
23 
26  return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping);
28  }
29 
31  const context::Model & model,
33  const context::VectorXs & tau,
34  const context::MatrixXs & J,
35  const context::VectorXs & gamma,
37  {
38 
41  return forwardDynamics(model, data, tau, J, gamma, inv_damping);
43  }
44 
46  const context::Model & model,
48  const context::VectorXs & q,
49  const context::VectorXs & v_before,
50  const context::MatrixXs & J,
53  {
54 
57  return impulseDynamics(model, data, q, v_before, J, r_coeff, inv_damping);
59  }
60 
62  const context::Model & model,
64  const context::VectorXs & v_before,
65  const context::MatrixXs & J,
68  {
69 
72  return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping);
74  }
75 
77  const context::Model & model,
79  const context::VectorXs & q,
80  const context::MatrixXs & J,
82  {
83  context::MatrixXs KKTMatrix_inv(model.nv + J.rows(), model.nv + J.rows());
85  return KKTMatrix_inv;
86  }
87 
90  {
91  context::MatrixXs MJtJ_inv(model.nv + J.rows(), model.nv + J.rows());
92 
97 
98  return MJtJ_inv;
99  }
100 
102  {
103  using namespace Eigen;
104 
105  bp::def(
106  "forwardDynamics", &forwardDynamics_proxy,
107  (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v"), bp::arg("tau"),
108  bp::arg("constraint_jacobian"), bp::arg("constraint_drift"), bp::arg("damping") = 0),
109  "Solves the constrained dynamics problem with contacts, puts the result in "
110  "context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
111  "Note: internally, pinocchio.computeAllTerms is called.");
112 
113  bp::def(
114  "forwardDynamics", &forwardDynamics_proxy_no_q,
115  (bp::arg("model"), bp::arg("data"), bp::arg("tau"), bp::arg("constraint_jacobian"),
116  bp::arg("constraint_drift"), bp::arg("damping") = 0),
117  "Solves the forward dynamics problem with contacts, puts the result in "
118  "context::Data::ddq and return it. The contact forces are stored in data.lambda_c.\n"
119  "Note: this function assumes that pinocchio.computeAllTerms has been called first.");
120 
121  bp::def(
122  "impulseDynamics", &impulseDynamics_proxy,
123  (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("v_before"),
124  bp::arg("constraint_jacobian"), bp::arg("restitution_coefficient") = 0,
125  bp::arg("damping") = 0),
126  "Solves the impact dynamics problem with contacts, store the result in "
127  "context::Data::dq_after and return it. The contact impulses are stored in "
128  "data.impulse_c.\n"
129  "Note: internally, pinocchio.crba is called.");
130 
131  bp::def(
132  "impulseDynamics", &impulseDynamics_proxy_no_q,
133  (bp::arg("model"), bp::arg("data"), bp::arg("v_before"), bp::arg("constraint_jacobian"),
134  bp::arg("restitution_coefficient") = 0, bp::arg("damping") = 0),
135  "Solves the impact dynamics problem with contacts, store the result in "
136  "context::Data::dq_after and return it. The contact impulses are stored in "
137  "data.impulse_c.\n"
138  "Note: this function assumes that pinocchio.crba has been called first.");
139 
140  bp::def(
141  "computeKKTContactDynamicMatrixInverse", computeKKTContactDynamicMatrixInverse_proxy,
142  (bp::arg("model"), bp::arg("data"), bp::arg("q"), bp::arg("constraint_jacobian"),
143  bp::arg("damping") = 0),
144  "Computes the inverse of the constraint matrix [[M J^T], [J 0]].");
145 
146  bp::def(
147  "getKKTContactDynamicMatrixInverse", getKKTContactDynamicMatrixInverse_proxy,
148  bp::args("model", "data", "constraint_jacobian"),
149  "Computes the inverse of the constraint matrix [[M Jt], [J 0]].\n forwardDynamics or "
150  "impulseDynamics must have been called first.\n"
151  "Note: the constraint Jacobian should be the same that was provided to "
152  "forwardDynamics or impulseDynamics.");
153  }
154 
155  } // namespace python
156 } // namespace pinocchio
pinocchio::getKKTContactDynamicMatrixInverse
PINOCCHIO_DEPRECATED 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.
Eigen
pinocchio::python::getKKTContactDynamicMatrixInverse_proxy
static const context::MatrixXs getKKTContactDynamicMatrixInverse_proxy(const context::Model &model, context::Data &data, const context::MatrixXs &J)
Definition: expose-contact-dynamics.cpp:88
pinocchio::python::forwardDynamics_proxy_no_q
static const context::VectorXs forwardDynamics_proxy_no_q(const context::Model &model, context::Data &data, const context::VectorXs &tau, const context::MatrixXs &J, const context::VectorXs &gamma, const context::Scalar inv_damping=context::Scalar(0.0))
Definition: expose-contact-dynamics.cpp:30
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::python::exposeContactDynamics
void exposeContactDynamics()
Definition: expose-contact-dynamics.cpp:101
pinocchio::python::impulseDynamics_proxy_no_q
static const context::VectorXs impulseDynamics_proxy_no_q(const context::Model &model, context::Data &data, const context::VectorXs &v_before, const context::MatrixXs &J, const context::Scalar r_coeff=context::Scalar(0.), const context::Scalar inv_damping=context::Scalar(0.))
Definition: expose-contact-dynamics.cpp:61
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::impulseDynamics_proxy
static const context::VectorXs impulseDynamics_proxy(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v_before, const context::MatrixXs &J, const context::Scalar r_coeff=context::Scalar(0.), const context::Scalar inv_damping=context::Scalar(0.))
Definition: expose-contact-dynamics.cpp:45
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Definition: include/pinocchio/macros.hpp:130
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
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...
bindings_dynamics.r_coeff
float r_coeff
Definition: bindings_dynamics.py:10
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
pinocchio::context::VectorXs
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
Definition: context/generic.hpp:47
algorithms.hpp
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
python
pinocchio::python::v
const Vector3Like & v
Definition: bindings/python/spatial/explog.hpp:66
pinocchio::python::computeKKTContactDynamicMatrixInverse_proxy
static context::MatrixXs computeKKTContactDynamicMatrixInverse_proxy(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::MatrixXs &J, const context::Scalar mu=context::Scalar(0))
Definition: expose-contact-dynamics.cpp:76
pinocchio::forwardDynamics
const PINOCCHIO_DEPRECATED 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...
bindings_dynamics.inv_damping
float inv_damping
Definition: bindings_dynamics.py:11
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: include/pinocchio/macros.hpp:131
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:129
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
mu
double mu
Definition: delassus.cpp:58
pinocchio::python::forwardDynamics_proxy
static const context::VectorXs forwardDynamics_proxy(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &tau, const context::MatrixXs &J, const context::VectorXs &gamma, const context::Scalar inv_damping=context::Scalar(0.0))
Definition: expose-contact-dynamics.cpp:13
pinocchio::impulseDynamics
const PINOCCHIO_DEPRECATED 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.
contact-dynamics.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::python::context::Scalar
PINOCCHIO_PYTHON_SCALAR_TYPE Scalar
Definition: bindings/python/context/generic.hpp:37
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:09