| 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 |