1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/rnea-derivatives.hpp" |
7 |
|
|
#include "pinocchio/bindings/python/utils/eigen.hpp" |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
namespace python |
12 |
|
|
{ |
13 |
|
|
|
14 |
|
|
namespace bp = boost::python; |
15 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector; |
16 |
|
|
|
17 |
|
|
Data::MatrixXs computeGeneralizedGravityDerivatives(const Model & model, Data & data, |
18 |
|
|
const Eigen::VectorXd & q) |
19 |
|
|
{ |
20 |
|
|
Data::MatrixXs res(model.nv,model.nv); |
21 |
|
|
res.setZero(); |
22 |
|
|
pinocchio::computeGeneralizedGravityDerivatives(model,data,q,res); |
23 |
|
|
return res; |
24 |
|
|
} |
25 |
|
|
|
26 |
|
|
Data::MatrixXs computeStaticTorqueDerivatives(const Model & model, Data & data, |
27 |
|
|
const Eigen::VectorXd & q, |
28 |
|
|
const ForceAlignedVector & fext) |
29 |
|
|
{ |
30 |
|
|
Data::MatrixXs res(model.nv,model.nv); |
31 |
|
|
res.setZero(); |
32 |
|
|
pinocchio::computeStaticTorqueDerivatives(model,data,q,fext,res); |
33 |
|
|
return res; |
34 |
|
|
} |
35 |
|
|
|
36 |
|
1 |
bp::tuple computeRNEADerivatives(const Model & model, Data & data, |
37 |
|
|
const Eigen::VectorXd & q, |
38 |
|
|
const Eigen::VectorXd & v, |
39 |
|
|
const Eigen::VectorXd & a) |
40 |
|
|
{ |
41 |
|
1 |
pinocchio::computeRNEADerivatives(model,data,q,v,a); |
42 |
|
1 |
make_symmetric(data.M); |
43 |
✓✗ |
1 |
return bp::make_tuple(make_ref(data.dtau_dq), |
44 |
✓✗ |
1 |
make_ref(data.dtau_dv), |
45 |
✓✗ |
3 |
make_ref(data.M)); |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
bp::tuple computeRNEADerivatives_fext(const Model & model, Data & data, |
49 |
|
|
const Eigen::VectorXd & q, |
50 |
|
|
const Eigen::VectorXd & v, |
51 |
|
|
const Eigen::VectorXd & a, |
52 |
|
|
const ForceAlignedVector & fext) |
53 |
|
|
{ |
54 |
|
|
pinocchio::computeRNEADerivatives(model,data,q,v,a,fext); |
55 |
|
|
make_symmetric(data.M); |
56 |
|
|
return bp::make_tuple(make_ref(data.dtau_dq), |
57 |
|
|
make_ref(data.dtau_dv), |
58 |
|
|
make_ref(data.M)); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
19 |
void exposeRNEADerivatives() |
62 |
|
|
{ |
63 |
|
|
using namespace Eigen; |
64 |
|
|
|
65 |
✓✗ |
19 |
bp::def("computeGeneralizedGravityDerivatives", |
66 |
|
|
computeGeneralizedGravityDerivatives, |
67 |
|
38 |
bp::args("model","data","q"), |
68 |
|
|
"Computes the partial derivative of the generalized gravity contribution\n" |
69 |
|
|
"with respect to the joint configuration.\n\n" |
70 |
|
|
"Parameters:\n" |
71 |
|
|
"\tmodel: model of the kinematic tree\n" |
72 |
|
|
"\tdata: data related to the model\n" |
73 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
74 |
|
|
"Returns: dtau_statique_dq\n"); |
75 |
|
|
|
76 |
✓✗ |
19 |
bp::def("computeStaticTorqueDerivatives", |
77 |
|
|
computeStaticTorqueDerivatives, |
78 |
|
38 |
bp::args("model","data","q","fext"), |
79 |
|
|
"Computes the partial derivative of the generalized gravity and external forces contributions (a.k.a static torque vector)\n" |
80 |
|
|
"with respect to the joint configuration.\n\n" |
81 |
|
|
"Parameters:\n" |
82 |
|
|
"\tmodel: model of the kinematic tree\n" |
83 |
|
|
"\tdata: data related to the model\n" |
84 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
85 |
|
|
"\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n" |
86 |
|
|
"Returns: dtau_statique_dq\n"); |
87 |
|
|
|
88 |
✓✗ |
19 |
bp::def("computeRNEADerivatives", |
89 |
|
|
computeRNEADerivatives, |
90 |
|
38 |
bp::args("model","data","q","v","a"), |
91 |
|
|
"Computes the RNEA partial derivatives, store the result in data.dtau_dq, data.dtau_dv and data.M (aka dtau_da)\n" |
92 |
|
|
"which correspond to the partial derivatives of the torque output with respect to the joint configuration,\n" |
93 |
|
|
"velocity and acceleration vectors.\n\n" |
94 |
|
|
"Parameters:\n" |
95 |
|
|
"\tmodel: model of the kinematic tree\n" |
96 |
|
|
"\tdata: data related to the model\n" |
97 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
98 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
99 |
|
|
"\ta: the joint acceleration vector (size model.nv)\n\n" |
100 |
|
|
"Returns: (dtau_dq, dtau_dv, dtau_da)\n"); |
101 |
|
|
|
102 |
✓✗ |
19 |
bp::def("computeRNEADerivatives", |
103 |
|
|
computeRNEADerivatives_fext, |
104 |
|
38 |
bp::args("model","data","q","v","a","fext"), |
105 |
|
|
"Computes the RNEA partial derivatives with external contact foces,\n" |
106 |
|
|
"store the result in data.dtau_dq, data.dtau_dv and data.M (aka dtau_da)\n" |
107 |
|
|
"which correspond to the partial derivatives of the torque output with respect to the joint configuration,\n" |
108 |
|
|
"velocity and acceleration vectors.\n\n" |
109 |
|
|
"Parameters:\n" |
110 |
|
|
"\tmodel: model of the kinematic tree\n" |
111 |
|
|
"\tdata: data related to the model\n" |
112 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
113 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
114 |
|
|
"\ta: the joint acceleration vector (size model.nv)\n" |
115 |
|
|
"\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n\n" |
116 |
|
|
"Returns: (dtau_dq, dtau_dv, dtau_da)\n"); |
117 |
|
19 |
} |
118 |
|
|
|
119 |
|
|
|
120 |
|
|
|
121 |
|
|
} // namespace python |
122 |
|
|
} // namespace pinocchio |