1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/aba-derivatives.hpp" |
7 |
|
|
#include "pinocchio/bindings/python/utils/eigen.hpp" |
8 |
|
|
|
9 |
|
|
#include <eigenpy/eigen-to-python.hpp> |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
namespace python |
14 |
|
|
{ |
15 |
|
|
|
16 |
|
|
namespace bp = boost::python; |
17 |
|
1 |
bp::tuple computeABADerivativesDefault(const Model & model, Data & data, |
18 |
|
|
const Eigen::VectorXd & q, |
19 |
|
|
const Eigen::VectorXd & v, |
20 |
|
|
const Eigen::VectorXd & tau) |
21 |
|
|
{ |
22 |
|
1 |
computeABADerivatives(model,data,q,v,tau); |
23 |
|
1 |
make_symmetric(data.Minv); |
24 |
✓✗ |
1 |
return bp::make_tuple(make_ref(data.ddq_dq), |
25 |
✓✗ |
1 |
make_ref(data.ddq_dv), |
26 |
✓✗ |
3 |
make_ref(data.Minv)); |
27 |
|
|
} |
28 |
|
|
|
29 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector; |
30 |
|
|
|
31 |
|
|
bp::tuple computeABADerivatives_fext(const Model & model, Data & data, |
32 |
|
|
const Eigen::VectorXd & q, |
33 |
|
|
const Eigen::VectorXd & v, |
34 |
|
|
const Eigen::VectorXd & tau, |
35 |
|
|
const ForceAlignedVector & fext) |
36 |
|
|
{ |
37 |
|
|
computeABADerivatives(model,data,q,v,tau,fext); |
38 |
|
|
make_symmetric(data.Minv); |
39 |
|
|
return bp::make_tuple(make_ref(data.ddq_dq), |
40 |
|
|
make_ref(data.ddq_dv), |
41 |
|
|
make_ref(data.Minv)); |
42 |
|
|
} |
43 |
|
|
|
44 |
|
19 |
void exposeABADerivatives() |
45 |
|
|
{ |
46 |
|
|
using namespace Eigen; |
47 |
|
|
|
48 |
✓✗ |
19 |
bp::def("computeABADerivatives", |
49 |
|
|
computeABADerivativesDefault, |
50 |
|
38 |
bp::args("model","data","q","v","tau"), |
51 |
|
|
"Computes the ABA derivatives, store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n" |
52 |
|
|
"which correspond to the partial derivatives of the joint acceleration vector output with respect to the joint configuration,\n" |
53 |
|
|
"velocity and torque vectors.\n\n" |
54 |
|
|
"Parameters:\n" |
55 |
|
|
"\tmodel: model of the kinematic tree\n" |
56 |
|
|
"\tdata: data related to the model\n" |
57 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
58 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
59 |
|
|
"\ttau: the joint torque vector (size model.nv)\n\n" |
60 |
|
|
"Returns: (ddq_dq, ddq_dv, ddq_da)"); |
61 |
|
|
|
62 |
✓✗ |
19 |
bp::def("computeABADerivatives", |
63 |
|
|
computeABADerivatives_fext, |
64 |
|
38 |
bp::args("model","data","q","v","tau","fext"), |
65 |
|
|
"Computes the ABA derivatives with external contact foces,\n" |
66 |
|
|
"store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n" |
67 |
|
|
"which correspond to the partial derivatives of the acceleration output with respect to the joint configuration,\n" |
68 |
|
|
"velocity and torque vectors.\n\n" |
69 |
|
|
"Parameters:\n" |
70 |
|
|
"\tmodel: model of the kinematic tree\n" |
71 |
|
|
"\tdata: data related to the model\n" |
72 |
|
|
"\tq: the joint configuration vector (size model.nq)\n" |
73 |
|
|
"\tv: the joint velocity vector (size model.nv)\n" |
74 |
|
|
"\ttau: the joint torque vector (size model.nv)\n" |
75 |
|
|
"\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n\n" |
76 |
|
|
"Returns: (ddq_dq, ddq_dv, ddq_da)"); |
77 |
|
19 |
} |
78 |
|
|
} // namespace python |
79 |
|
|
} // namespace pinocchio |