1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019 CNRS |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/centroidal-derivatives.hpp" |
7 |
|
|
|
8 |
|
|
#include <boost/python/tuple.hpp> |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
namespace python |
13 |
|
|
{ |
14 |
|
|
|
15 |
|
|
bp::tuple computeCentroidalDynamicsDerivatives_proxy(const Model & model, |
16 |
|
|
Data & data, |
17 |
|
|
const Eigen::VectorXd & q, |
18 |
|
|
const Eigen::VectorXd & v, |
19 |
|
|
const Eigen::VectorXd & a) |
20 |
|
|
{ |
21 |
|
|
typedef Data::Matrix6x Matrix6x; |
22 |
|
|
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv)); |
23 |
|
|
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv)); |
24 |
|
|
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv)); |
25 |
|
|
Matrix6x partial_da(Matrix6x::Zero(6,model.nv)); |
26 |
|
|
|
27 |
|
|
computeCentroidalDynamicsDerivatives(model,data,q, v, a, |
28 |
|
|
partialh_dq, partial_dq, partial_dv, partial_da); |
29 |
|
|
|
30 |
|
|
return bp::make_tuple(partialh_dq, partial_dq,partial_dv,partial_da); |
31 |
|
|
} |
32 |
|
|
|
33 |
|
|
bp::tuple getCentroidalDynamicsDerivatives_proxy(const Model & model, |
34 |
|
|
Data & data) |
35 |
|
|
{ |
36 |
|
|
typedef Data::Matrix6x Matrix6x; |
37 |
|
|
|
38 |
|
|
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv)); |
39 |
|
|
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv)); |
40 |
|
|
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv)); |
41 |
|
|
Matrix6x partial_da(Matrix6x::Zero(6,model.nv)); |
42 |
|
|
|
43 |
|
|
getCentroidalDynamicsDerivatives(model,data, partialh_dq, partial_dq, partial_dv, partial_da); |
44 |
|
|
return bp::make_tuple(partialh_dq,partial_dq,partial_dv,partial_da); |
45 |
|
|
} |
46 |
|
|
|
47 |
|
19 |
void exposeCentroidalDerivatives() |
48 |
|
|
{ |
49 |
|
|
using namespace Eigen; |
50 |
|
|
|
51 |
✓✗ |
19 |
bp::def("computeCentroidalDynamicsDerivatives", |
52 |
|
|
computeCentroidalDynamicsDerivatives_proxy, |
53 |
|
38 |
bp::args("Model","Data","q","v","a"), |
54 |
|
|
"Computes the analytical derivatives of the centroidal dynamics\n" |
55 |
|
|
"with respect to the joint configuration vector, velocity and acceleration."); |
56 |
|
|
|
57 |
✓✗ |
19 |
bp::def("getCentroidalDynamicsDerivatives", |
58 |
|
|
getCentroidalDynamicsDerivatives_proxy, |
59 |
|
38 |
bp::args("Model","Data"), |
60 |
|
|
"Retrive the analytical derivatives of the centroidal dynamics\n" |
61 |
|
|
"from the RNEA derivatives.\n" |
62 |
|
|
"pinocchio.computeRNEADerivatives should have been called first."); |
63 |
|
19 |
} |
64 |
|
|
|
65 |
|
|
} // namespace python |
66 |
|
|
} // namespace pinocchio |