1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2020 CNRS, INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/contact-dynamics.hpp" |
7 |
|
|
|
8 |
|
|
namespace pinocchio |
9 |
|
|
{ |
10 |
|
|
namespace python |
11 |
|
|
{ |
12 |
|
|
|
13 |
|
1 |
static const Eigen::VectorXd forwardDynamics_proxy(const Model & model, |
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 |
|
1 |
return forwardDynamics(model, data, q, v, tau, J, gamma, inv_damping); |
23 |
|
|
} |
24 |
|
|
|
25 |
|
40 |
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads, forwardDynamics_proxy, 7, 8) |
26 |
|
|
|
27 |
|
|
static const Eigen::VectorXd forwardDynamics_proxy_no_q(const Model & model, |
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 |
|
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(forwardDynamics_overloads_no_q, forwardDynamics_proxy_no_q, 5, 6) |
38 |
|
|
|
39 |
|
|
static const Eigen::VectorXd impulseDynamics_proxy(const Model & model, |
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 |
|
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads, impulseDynamics_proxy, 5, 7) |
51 |
|
|
|
52 |
|
|
static const Eigen::VectorXd impulseDynamics_proxy_no_q(const Model & model, |
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 |
|
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6) |
63 |
|
|
|
64 |
|
|
static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model & model, |
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 |
|
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, |
76 |
|
|
computeKKTContactDynamicMatrixInverse_proxy, 4, 5) |
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 |
|
|
|
87 |
|
19 |
void exposeDynamics() |
88 |
|
|
{ |
89 |
|
|
using namespace Eigen; |
90 |
|
|
|
91 |
✓✗ |
19 |
bp::def("forwardDynamics", |
92 |
|
|
&forwardDynamics_proxy, |
93 |
✓✗ |
19 |
forwardDynamics_overloads( |
94 |
|
38 |
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 |
✓✗ |
19 |
bp::def("forwardDynamics", |
100 |
|
|
&forwardDynamics_proxy_no_q, |
101 |
✓✗ |
19 |
forwardDynamics_overloads_no_q( |
102 |
|
38 |
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 |
✓✗ |
19 |
bp::def("impulseDynamics", |
108 |
|
|
&impulseDynamics_proxy, |
109 |
✓✗ |
19 |
impulseDynamics_overloads( |
110 |
|
38 |
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 |
✓✗ |
19 |
bp::def("impulseDynamics", |
116 |
|
|
&impulseDynamics_proxy_no_q, |
117 |
✓✗ |
19 |
impulseDynamics_overloads_no_q( |
118 |
|
38 |
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 |
✓✗ |
19 |
bp::def("computeKKTContactDynamicMatrixInverse", |
124 |
|
|
computeKKTContactDynamicMatrixInverse_proxy, |
125 |
✓✗ |
38 |
computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","J","damping"), |
126 |
|
|
"Computes the inverse of the constraint matrix [[M J^T], [J 0]].")); |
127 |
|
|
|
128 |
✓✗ |
19 |
bp::def("getKKTContactDynamicMatrixInverse", |
129 |
|
|
getKKTContactDynamicMatrixInverse_proxy, |
130 |
|
38 |
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 |
|
19 |
} |
133 |
|
|
|
134 |
|
|
} // namespace python |
135 |
|
|
} // namespace pinocchio |