1 |
|
|
// |
2 |
|
|
// Copyright (c) 2020 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/algorithm/algorithms.hpp" |
6 |
|
|
#include "pinocchio/algorithm/frames-derivatives.hpp" |
7 |
|
|
|
8 |
|
|
#include <boost/python/tuple.hpp> |
9 |
|
|
|
10 |
|
|
namespace pinocchio |
11 |
|
|
{ |
12 |
|
|
namespace python |
13 |
|
|
{ |
14 |
|
|
namespace bp = boost::python; |
15 |
|
|
|
16 |
|
|
bp::tuple getFrameVelocityDerivatives_proxy(const Model & model, |
17 |
|
|
Data & data, |
18 |
|
|
const Model::FrameIndex frame_id, |
19 |
|
|
ReferenceFrame rf) |
20 |
|
|
{ |
21 |
|
|
typedef Data::Matrix6x Matrix6x; |
22 |
|
|
|
23 |
|
|
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv)); |
24 |
|
|
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv)); |
25 |
|
|
|
26 |
|
|
getFrameVelocityDerivatives(model,data,frame_id,rf, |
27 |
|
|
partial_dq,partial_dv); |
28 |
|
|
|
29 |
|
|
return bp::make_tuple(partial_dq,partial_dv); |
30 |
|
|
} |
31 |
|
|
|
32 |
|
|
bp::tuple getFrameAccelerationDerivatives_proxy(const Model & model, |
33 |
|
|
Data & data, |
34 |
|
|
const Model::FrameIndex frame_id, |
35 |
|
|
ReferenceFrame rf) |
36 |
|
|
{ |
37 |
|
|
typedef Data::Matrix6x Matrix6x; |
38 |
|
|
|
39 |
|
|
Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv)); |
40 |
|
|
Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv)); |
41 |
|
|
Matrix6x a_partial_dv(Matrix6x::Zero(6,model.nv)); |
42 |
|
|
Matrix6x a_partial_da(Matrix6x::Zero(6,model.nv)); |
43 |
|
|
|
44 |
|
|
getFrameAccelerationDerivatives(model,data,frame_id,rf, |
45 |
|
|
v_partial_dq,a_partial_dq, |
46 |
|
|
a_partial_dv,a_partial_da); |
47 |
|
|
|
48 |
|
|
return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da); |
49 |
|
|
} |
50 |
|
|
|
51 |
|
19 |
void exposeFramesDerivatives() |
52 |
|
|
{ |
53 |
|
|
using namespace Eigen; |
54 |
|
|
|
55 |
✓✗ |
19 |
bp::def("getFrameVelocityDerivatives", |
56 |
|
|
getFrameVelocityDerivatives_proxy, |
57 |
|
38 |
bp::args("model","data","frame_id","reference_frame"), |
58 |
|
|
"Computes the partial derivatives of the spatial velocity of a given frame with respect to\n" |
59 |
|
|
"the joint configuration and velocity and returns them as a tuple.\n" |
60 |
|
|
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n" |
61 |
|
|
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n" |
62 |
|
|
"Parameters:\n" |
63 |
|
|
"\tmodel: model of the kinematic tree\n" |
64 |
|
|
"\tdata: data related to the model\n" |
65 |
|
|
"\tframe_id: index of the frame\n" |
66 |
|
|
"\treference_frame: reference frame in which the resulting derivatives are expressed\n"); |
67 |
|
|
|
68 |
✓✗ |
19 |
bp::def("getFrameAccelerationDerivatives", |
69 |
|
|
getFrameAccelerationDerivatives_proxy, |
70 |
|
38 |
bp::args("model","data","frame_id","reference_frame"), |
71 |
|
|
"Computes the partial derivatives of the spatial acceleration of a given frame with respect to\n" |
72 |
|
|
"the joint configuration, velocity and acceleration and returns them as a tuple.\n" |
73 |
|
|
"The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n" |
74 |
|
|
"You must first call computeForwardKinematicsDerivatives before calling this function.\n\n" |
75 |
|
|
"Parameters:\n" |
76 |
|
|
"\tmodel: model of the kinematic tree\n" |
77 |
|
|
"\tdata: data related to the model\n" |
78 |
|
|
"\tframe_id: index of the frame\n" |
79 |
|
|
"\treference_frame: reference frame in which the resulting derivatives are expressed\n"); |
80 |
|
|
|
81 |
|
19 |
} |
82 |
|
|
|
83 |
|
|
} // namespace python |
84 |
|
|
} // namespace pinocchio |