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