GCC Code Coverage Report

Directory: ./
File: bindings/python/math/expose-rpy.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 37 37 100.0%
Branches: 47 100 47.0%
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 10 context::Matrix3s rotate(const std::string & axis, const context::Scalar ang)
16 {
17 typedef context::Scalar Scalar;
18
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 9 times.
 10 if (axis.length() != 1U)
19
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
20
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 context::Vector3s u;
21
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 u.setZero();
22 9 const char axis_ = axis[0];
23
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 9 switch (axis_)
24 {
25 4 case 'x':
26
1/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 4 u[0] = Scalar(1);
27 4 break;
28 3 case 'y':
29
1/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 3 u[1] = Scalar(1);
30 3 break;
31 1 case 'z':
32
1/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1 u[2] = Scalar(1);
33 1 break;
34 1 default:
35
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 throw std::invalid_argument(std::string("Invalid axis: ").append(1U, axis_));
36 }
37
38
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 return context::AngleAxis(ang, u).matrix();
39 }
40
41 69 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 69 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 69 times.
✗ Branch 9 not taken.
 138 bp::scope current_scope = getOrCreatePythonNamespace("rpy");
49
50
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 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 69 times.
✗ Branch 2 not taken.
 138 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 69 times.
✗ Branch 2 not taken.
 69 bp::def(
60 "rpyToMatrix",
61 static_cast<context::Matrix3s (*)(const MatrixBase<context::Vector3s> &)>(&rpyToMatrix),
62
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 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 69 times.
✗ Branch 2 not taken.
 69 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 69 times.
✗ Branch 2 not taken.
 138 &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 69 times.
✗ Branch 2 not taken.
 69 bp::def(
78
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "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 69 times.
✗ Branch 2 not taken.
 69 bp::def(
83 "computeRpyJacobian", &computeRpyJacobian<context::Vector3s>,
84
4/8
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
 138 (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 69 times.
✗ Branch 2 not taken.
 69 bp::def(
97 "computeRpyJacobianInverse", &computeRpyJacobianInverse<context::Vector3s>,
98
4/8
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
 138 (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 69 times.
✗ Branch 2 not taken.
 69 bp::def(
111 "computeRpyJacobianTimeDerivative",
112 &computeRpyJacobianTimeDerivative<context::Vector3s, context::Vector3s>,
113
6/12
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 69 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 69 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 69 times.
✗ Branch 17 not taken.
 138 (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 69 }
125 69 }
126
127 } // namespace python
128 } // namespace pinocchio
129