1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/bindings/python/fwd.hpp" |
6 |
|
|
#include "pinocchio/bindings/python/utils/namespace.hpp" |
7 |
|
|
#include "pinocchio/math/rpy.hpp" |
8 |
|
|
|
9 |
|
|
namespace pinocchio |
10 |
|
|
{ |
11 |
|
|
namespace python |
12 |
|
|
{ |
13 |
|
|
namespace bp = boost::python; |
14 |
|
|
|
15 |
|
38 |
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2) |
16 |
|
19 |
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2) |
17 |
|
19 |
BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3) |
18 |
|
|
|
19 |
|
|
Eigen::Matrix3d rotate(const std::string & axis, const double ang) |
20 |
|
|
{ |
21 |
|
|
if(axis.length() != 1U) |
22 |
|
|
throw std::invalid_argument(std::string("Invalid axis: ").append(axis)); |
23 |
|
|
Eigen::Vector3d u; |
24 |
|
|
u.setZero(); |
25 |
|
|
const char axis_ = axis[0]; |
26 |
|
|
switch(axis_) |
27 |
|
|
{ |
28 |
|
|
case 'x': u[0] = 1.; break; |
29 |
|
|
case 'y': u[1] = 1.; break; |
30 |
|
|
case 'z': u[2] = 1.; break; |
31 |
|
|
default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_)); |
32 |
|
|
} |
33 |
|
|
|
34 |
|
|
return Eigen::AngleAxisd(ang, u).matrix(); |
35 |
|
|
} |
36 |
|
|
|
37 |
|
19 |
void exposeRpy() |
38 |
|
|
{ |
39 |
|
|
using namespace Eigen; |
40 |
|
|
using namespace pinocchio::rpy; |
41 |
|
|
|
42 |
|
|
{ |
43 |
|
|
// using the rpy scope |
44 |
✓✗✓✗
✓✗ |
38 |
bp::scope current_scope = getOrCreatePythonNamespace("rpy"); |
45 |
|
|
|
46 |
✓✗ |
19 |
bp::def("rpyToMatrix", |
47 |
|
|
static_cast<Matrix3d (*)(const double&, const double&, const double&)>(&rpyToMatrix), |
48 |
✓✗ |
38 |
bp::args("roll", "pitch", "yaw"), |
49 |
|
|
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," |
50 |
|
|
" where R_a(theta) denotes the rotation of theta radians axis a"); |
51 |
|
|
|
52 |
✓✗ |
19 |
bp::def("rpyToMatrix", |
53 |
|
|
static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix), |
54 |
✓✗ |
38 |
bp::arg("rpy"), |
55 |
|
|
"Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," |
56 |
|
|
" where R_a(theta) denotes the rotation of theta radians axis a"); |
57 |
|
|
|
58 |
✓✗ |
19 |
bp::def("matrixToRpy", |
59 |
|
|
&matrixToRpy<Matrix3d>, |
60 |
✓✗ |
38 |
bp::arg("R"), |
61 |
|
|
"Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r)," |
62 |
|
|
" where R_a(theta) denotes the rotation of theta radians axis a." |
63 |
|
|
" The angles are guaranteed to be in the ranges: r in [-pi,pi]," |
64 |
|
|
" p in[-pi/2,pi/2], y in [-pi,pi]"); |
65 |
|
|
|
66 |
✓✗ |
19 |
bp::def("rotate", |
67 |
|
|
&rotate, |
68 |
✓✗ |
38 |
bp::args("axis", "ang"), |
69 |
|
|
"Rotation matrix corresponding to a rotation about x, y or z" |
70 |
|
|
" e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis"); |
71 |
|
|
|
72 |
✓✗ |
19 |
bp::def("computeRpyJacobian", |
73 |
|
|
&computeRpyJacobian<Vector3d>, |
74 |
✓✗ |
19 |
computeRpyJacobian_overload( |
75 |
✓✗ |
38 |
bp::args("rpy","reference_frame"), |
76 |
|
|
"Compute the Jacobian of the Roll-Pitch-Yaw conversion" |
77 |
|
|
" Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" |
78 |
|
|
" and reference frame F (either LOCAL or WORLD)," |
79 |
|
|
" the Jacobian is such that omega_F = J_F(phi)phidot," |
80 |
|
|
" where omega_F is the angular velocity expressed in frame F" |
81 |
|
|
" and J_F is the Jacobian computed with reference frame F" |
82 |
|
|
"\nParameters:\n" |
83 |
|
|
"\trpy Roll-Pitch-Yaw vector" |
84 |
|
|
"\treference_frame Reference frame in which the angular velocity is expressed." |
85 |
|
|
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" |
86 |
|
|
) |
87 |
|
|
); |
88 |
|
|
|
89 |
✓✗ |
19 |
bp::def("computeRpyJacobianInverse", |
90 |
|
|
&computeRpyJacobianInverse<Vector3d>, |
91 |
✓✗ |
19 |
computeRpyJacobianInverse_overload( |
92 |
✓✗ |
38 |
bp::args("rpy","reference_frame"), |
93 |
|
|
"Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion" |
94 |
|
|
" Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" |
95 |
|
|
" and reference frame F (either LOCAL or WORLD)," |
96 |
|
|
" the Jacobian is such that omega_F = J_F(phi)phidot," |
97 |
|
|
" where omega_F is the angular velocity expressed in frame F" |
98 |
|
|
" and J_F is the Jacobian computed with reference frame F" |
99 |
|
|
"\nParameters:\n" |
100 |
|
|
"\trpy Roll-Pitch-Yaw vector" |
101 |
|
|
"\treference_frame Reference frame in which the angular velocity is expressed." |
102 |
|
|
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" |
103 |
|
|
) |
104 |
|
|
); |
105 |
|
|
|
106 |
✓✗ |
19 |
bp::def("computeRpyJacobianTimeDerivative", |
107 |
|
|
&computeRpyJacobianTimeDerivative<Vector3d, Vector3d>, |
108 |
✓✗ |
19 |
computeRpyJacobianTimeDerivative_overload( |
109 |
✓✗ |
38 |
bp::args("rpy", "rpydot", "reference_frame"), |
110 |
|
|
"Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion" |
111 |
|
|
" Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" |
112 |
|
|
" and reference frame F (either LOCAL or WORLD)," |
113 |
|
|
" the Jacobian is such that omega_F = J_F(phi)phidot," |
114 |
|
|
" where omega_F is the angular velocity expressed in frame F" |
115 |
|
|
" and J_F is the Jacobian computed with reference frame F" |
116 |
|
|
"\nParameters:\n" |
117 |
|
|
"\trpy Roll-Pitch-Yaw vector" |
118 |
|
|
"\treference_frame Reference frame in which the angular velocity is expressed." |
119 |
|
|
" Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" |
120 |
|
|
) |
121 |
|
|
); |
122 |
|
|
} |
123 |
|
|
|
124 |
|
19 |
} |
125 |
|
|
|
126 |
|
|
} // namespace python |
127 |
|
|
} // namespace pinocchio |