Directory: | ./ |
---|---|
File: | bindings/python/algorithm/expose-rnea-derivatives.cpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 14 | 28 | 50.0% |
Branches: | 7 | 28 | 25.0% |
Line | Branch | Exec | Source |
---|---|---|---|
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(context::Force) ForceAlignedVector; | ||
16 | |||
17 | ✗ | context::Data::MatrixXs computeGeneralizedGravityDerivatives( | |
18 | const context::Model & model, context::Data & data, const context::VectorXs & q) | ||
19 | { | ||
20 | ✗ | context::Data::MatrixXs res(model.nv, model.nv); | |
21 | ✗ | res.setZero(); | |
22 | ✗ | pinocchio::computeGeneralizedGravityDerivatives(model, data, q, res); | |
23 | ✗ | return res; | |
24 | } | ||
25 | |||
26 | ✗ | context::Data::MatrixXs computeStaticTorqueDerivatives( | |
27 | const context::Model & model, | ||
28 | context::Data & data, | ||
29 | const context::VectorXs & q, | ||
30 | const ForceAlignedVector & fext) | ||
31 | { | ||
32 | ✗ | context::Data::MatrixXs res(model.nv, model.nv); | |
33 | ✗ | res.setZero(); | |
34 | ✗ | pinocchio::computeStaticTorqueDerivatives(model, data, q, fext, res); | |
35 | ✗ | return res; | |
36 | } | ||
37 | |||
38 | 1 | bp::tuple computeRNEADerivatives( | |
39 | const context::Model & model, | ||
40 | context::Data & data, | ||
41 | const context::VectorXs & q, | ||
42 | const context::VectorXs & v, | ||
43 | const context::VectorXs & a) | ||
44 | { | ||
45 | 1 | pinocchio::computeRNEADerivatives(model, data, q, v, a); | |
46 | 1 | make_symmetric(data.M); | |
47 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | return bp::make_tuple(make_ref(data.dtau_dq), make_ref(data.dtau_dv), make_ref(data.M)); |
48 | } | ||
49 | |||
50 | ✗ | bp::tuple computeRNEADerivatives_fext( | |
51 | const context::Model & model, | ||
52 | context::Data & data, | ||
53 | const context::VectorXs & q, | ||
54 | const context::VectorXs & v, | ||
55 | const context::VectorXs & a, | ||
56 | const ForceAlignedVector & fext) | ||
57 | { | ||
58 | ✗ | pinocchio::computeRNEADerivatives(model, data, q, v, a, fext); | |
59 | ✗ | make_symmetric(data.M); | |
60 | ✗ | return bp::make_tuple(make_ref(data.dtau_dq), make_ref(data.dtau_dv), make_ref(data.M)); | |
61 | } | ||
62 | |||
63 | 20 | void exposeRNEADerivatives() | |
64 | { | ||
65 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
66 | "computeGeneralizedGravityDerivatives", computeGeneralizedGravityDerivatives, | ||
67 | 40 | 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 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
77 | "computeStaticTorqueDerivatives", computeStaticTorqueDerivatives, | ||
78 | 40 | bp::args("model", "data", "q", "fext"), | |
79 | "Computes the partial derivative of the generalized gravity and external forces " | ||
80 | "contributions (a.k.a static torque vector)\n" | ||
81 | "with respect to the joint configuration.\n\n" | ||
82 | "Parameters:\n" | ||
83 | "\tmodel: model of the kinematic tree\n" | ||
84 | "\tdata: data related to the model\n" | ||
85 | "\tq: the joint configuration vector (size model.nq)\n" | ||
86 | "\tfext: list of external forces expressed in the local frame of the joints (size " | ||
87 | "model.njoints)\n" | ||
88 | "Returns: dtau_statique_dq\n"); | ||
89 | |||
90 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
91 | 40 | "computeRNEADerivatives", computeRNEADerivatives, bp::args("model", "data", "q", "v", "a"), | |
92 | "Computes the RNEA partial derivatives, store the result in data.dtau_dq, " | ||
93 | "data.dtau_dv and data.M (aka dtau_da)\n" | ||
94 | "which correspond to the partial derivatives of the torque output with respect to " | ||
95 | "the joint configuration,\n" | ||
96 | "velocity and acceleration vectors.\n\n" | ||
97 | "Parameters:\n" | ||
98 | "\tmodel: model of the kinematic tree\n" | ||
99 | "\tdata: data related to the model\n" | ||
100 | "\tq: the joint configuration vector (size model.nq)\n" | ||
101 | "\tv: the joint velocity vector (size model.nv)\n" | ||
102 | "\ta: the joint acceleration vector (size model.nv)\n\n" | ||
103 | "Returns: (dtau_dq, dtau_dv, dtau_da)\n"); | ||
104 | |||
105 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | bp::def( |
106 | "computeRNEADerivatives", computeRNEADerivatives_fext, | ||
107 | 40 | bp::args("model", "data", "q", "v", "a", "fext"), | |
108 | "Computes the RNEA partial derivatives with external contact foces,\n" | ||
109 | "store the result in data.dtau_dq, data.dtau_dv and data.M (aka dtau_da)\n" | ||
110 | "which correspond to the partial derivatives of the torque output with respect to " | ||
111 | "the joint configuration,\n" | ||
112 | "velocity and acceleration vectors.\n\n" | ||
113 | "Parameters:\n" | ||
114 | "\tmodel: model of the kinematic tree\n" | ||
115 | "\tdata: data related to the model\n" | ||
116 | "\tq: the joint configuration vector (size model.nq)\n" | ||
117 | "\tv: the joint velocity vector (size model.nv)\n" | ||
118 | "\ta: the joint acceleration vector (size model.nv)\n" | ||
119 | "\tfext: list of external forces expressed in the local frame of the joints (size " | ||
120 | "model.njoints)\n\n" | ||
121 | "Returns: (dtau_dq, dtau_dv, dtau_da)\n"); | ||
122 | 20 | } | |
123 | |||
124 | } // namespace python | ||
125 | } // namespace pinocchio | ||
126 |