|
| 1 | +/* Exercise the posemath conversions exported by tpmod. |
| 2 | + * |
| 3 | + * The oracle constructs Rz(yaw) Ry(pitch) Rx(roll) independently of |
| 4 | + * posemath. At a pole the Euler angles are not unique, so check both |
| 5 | + * the reconstructed rotation and posemath's canonical representation. |
| 6 | + */ |
| 7 | +#include <rtapi.h> |
| 8 | +#include <rtapi_app.h> |
| 9 | +#include <rtapi_math.h> |
| 10 | +#include <hal.h> |
| 11 | +#include <posemath.h> |
| 12 | + |
| 13 | +MODULE_LICENSE("GPL"); |
| 14 | +MODULE_DESCRIPTION("posemath Euler singularity regression checks"); |
| 15 | + |
| 16 | +#define TIGHT 1.0e-12 |
| 17 | + |
| 18 | +typedef double matrix[3][3]; |
| 19 | +static int comp_id = -1; |
| 20 | +static int failures; |
| 21 | + |
| 22 | +static void multiply(const matrix a, const matrix b, matrix result) |
| 23 | +{ |
| 24 | + int i, j, k; |
| 25 | + for (i = 0; i < 3; ++i) |
| 26 | + for (j = 0; j < 3; ++j) { |
| 27 | + result[i][j] = 0; |
| 28 | + for (k = 0; k < 3; ++k) |
| 29 | + result[i][j] += a[i][k] * b[k][j]; |
| 30 | + } |
| 31 | +} |
| 32 | + |
| 33 | +static void rotation(double roll, double pitch, double yaw, matrix result) |
| 34 | +{ |
| 35 | + matrix rx = {{1, 0, 0}, {0, cos(roll), -sin(roll)}, |
| 36 | + {0, sin(roll), cos(roll)}}; |
| 37 | + matrix ry = {{cos(pitch), 0, sin(pitch)}, {0, 1, 0}, |
| 38 | + {-sin(pitch), 0, cos(pitch)}}; |
| 39 | + matrix rz = {{cos(yaw), -sin(yaw), 0}, {sin(yaw), cos(yaw), 0}, |
| 40 | + {0, 0, 1}}; |
| 41 | + matrix temp; |
| 42 | + multiply(rz, ry, temp); |
| 43 | + multiply(temp, rx, result); |
| 44 | +} |
| 45 | + |
| 46 | +static void fail(const char *name, const char *api, const char *what, |
| 47 | + double actual, double expected) |
| 48 | +{ |
| 49 | + rtapi_print_msg(RTAPI_MSG_ERR, |
| 50 | + "eulercheck: FAIL %s, %s: %s %.17g (expected %.17g)\n", |
| 51 | + name, api, what, actual, expected); |
| 52 | + ++failures; |
| 53 | +} |
| 54 | + |
| 55 | +static void check_result(const char *name, const char *api, int status, |
| 56 | + double roll, double pitch, double yaw, |
| 57 | + const matrix input, int pole, double expected_roll, |
| 58 | + double tolerance, int outside, double fuzz) |
| 59 | +{ |
| 60 | + matrix result; |
| 61 | + double error = 0; |
| 62 | + int i, j; |
| 63 | + if (status != PM_OK) { |
| 64 | + fail(name, api, "return code", status, PM_OK); |
| 65 | + return; |
| 66 | + } |
| 67 | + if (!isfinite(roll) || !isfinite(pitch) || !isfinite(yaw)) { |
| 68 | + rtapi_print_msg(RTAPI_MSG_ERR, |
| 69 | + "eulercheck: FAIL %s, %s: non-finite angles (%g, %g, %g)\n", |
| 70 | + name, api, roll, pitch, yaw); |
| 71 | + ++failures; |
| 72 | + return; |
| 73 | + } |
| 74 | + rotation(roll, pitch, yaw, result); |
| 75 | + for (i = 0; i < 3; ++i) |
| 76 | + for (j = 0; j < 3; ++j) |
| 77 | + error = fmax(error, fabs(result[i][j] - input[i][j])); |
| 78 | + if (error > tolerance) |
| 79 | + fail(name, api, "matrix error exceeds tolerance:", error, tolerance); |
| 80 | + |
| 81 | + if (pole) { |
| 82 | + /* Both fields are assigned exactly by the singularity branch. */ |
| 83 | + double canonical_pitch = pole * PM_PI_2; |
| 84 | + if (pitch != canonical_pitch) |
| 85 | + fail(name, api, "canonical pitch", pitch, canonical_pitch); |
| 86 | + if (yaw != 0) |
| 87 | + fail(name, api, "canonical yaw", yaw, 0); |
| 88 | + error = fabs(atan2(sin(roll - expected_roll), |
| 89 | + cos(roll - expected_roll))); |
| 90 | + if (error > TIGHT) |
| 91 | + fail(name, api, "roll error modulo 2*pi:", error, TIGHT); |
| 92 | + } |
| 93 | + if (outside) { |
| 94 | + double distance = PM_PI_2 - fabs(pitch); |
| 95 | + if (distance <= fuzz) |
| 96 | + fail(name, api, "outside pitch distance must exceed fuzz:", |
| 97 | + distance, fuzz); |
| 98 | + if (fabs(yaw) < TIGHT) |
| 99 | + fail(name, api, "outside yaw unexpectedly snapped:", yaw, TIGHT); |
| 100 | + } |
| 101 | +} |
| 102 | + |
| 103 | +static void check_matrix(const char *name, const matrix input, int pole, |
| 104 | + double expected_roll, double tolerance, |
| 105 | + int outside, int use_zyx, double fuzz) |
| 106 | +{ |
| 107 | + /* PmRotationMatrix stores columns; the oracle uses row-major arrays. */ |
| 108 | + PmRotationMatrix m = {{input[0][0], input[1][0], input[2][0]}, |
| 109 | + {input[0][1], input[1][1], input[2][1]}, |
| 110 | + {input[0][2], input[1][2], input[2][2]}}; |
| 111 | + int status; |
| 112 | + if (use_zyx) { |
| 113 | + PmEulerZyx zyx = {nan(""), nan(""), nan("")}; |
| 114 | + status = pmMatZyxConvert(&m, &zyx); |
| 115 | + check_result(name, "pmMatZyxConvert", status, zyx.x, zyx.y, zyx.z, |
| 116 | + input, pole, expected_roll, tolerance, outside, fuzz); |
| 117 | + } else { |
| 118 | + PmRpy rpy = {nan(""), nan(""), nan("")}; |
| 119 | + status = pmMatRpyConvert(&m, &rpy); |
| 120 | + check_result(name, "pmMatRpyConvert", status, rpy.r, rpy.p, rpy.y, |
| 121 | + input, pole, expected_roll, tolerance, outside, fuzz); |
| 122 | + } |
| 123 | +} |
| 124 | + |
| 125 | +static void check_quaternion(const char *name, const matrix input, int pole, |
| 126 | + double roll, int sign, int use_zyx, double fuzz) |
| 127 | +{ |
| 128 | + /* Ry(pole*pi/2) Rx(roll): analytically normalized, without posemath. */ |
| 129 | + double c = sign * cos(roll / 2) / sqrt(2.0); |
| 130 | + double s = sign * sin(roll / 2) / sqrt(2.0); |
| 131 | + PmQuaternion q = {c, s, pole * c, -pole * s}; |
| 132 | + double norm = q.s * q.s + q.x * q.x + q.y * q.y + q.z * q.z; |
| 133 | + int status; |
| 134 | + if (fabs(norm - 1) > TIGHT) |
| 135 | + fail(name, "quaternion input", "squared norm", norm, 1); |
| 136 | + if (use_zyx) { |
| 137 | + PmEulerZyx zyx = {nan(""), nan(""), nan("")}; |
| 138 | + status = pmQuatZyxConvert(&q, &zyx); |
| 139 | + check_result(name, "pmQuatZyxConvert", status, zyx.x, zyx.y, zyx.z, |
| 140 | + input, pole, roll, TIGHT, 0, fuzz); |
| 141 | + } else { |
| 142 | + PmRpy rpy = {nan(""), nan(""), nan("")}; |
| 143 | + status = pmQuatRpyConvert(&q, &rpy); |
| 144 | + check_result(name, "pmQuatRpyConvert", status, rpy.r, rpy.p, rpy.y, |
| 145 | + input, pole, roll, TIGHT, 0, fuzz); |
| 146 | + } |
| 147 | +} |
| 148 | + |
| 149 | +int rtapi_app_main(void) |
| 150 | +{ |
| 151 | + /* Sum/difference angles cover all quadrants and wrap past +/-pi. */ |
| 152 | + static const double pairs[][2] = { |
| 153 | + {30, 20}, {120, 25}, {-125, -40}, {-40, 15}, |
| 154 | + {170, 40}, {-150, 60}, {91.377, 88.623} |
| 155 | + }; |
| 156 | + static const double offsets[] = {0.25, 0.5, 2, 10}; |
| 157 | + static const double ordinary[][3] = { |
| 158 | + {0, 0, 0}, {30, 20, -40}, {-170, -60, 135}, |
| 159 | + {143, 67, -112}, {-85, 42, 178}, {75, -35, -125}, |
| 160 | + {180, 0, -90} |
| 161 | + }; |
| 162 | + unsigned int i, j; |
| 163 | + int use_zyx, pole, side, sign; |
| 164 | + char name[160]; |
| 165 | + matrix input; |
| 166 | + |
| 167 | + comp_id = hal_init("eulercheck"); |
| 168 | + if (comp_id < 0) |
| 169 | + return comp_id; |
| 170 | + |
| 171 | + for (use_zyx = 0; use_zyx <= 1; ++use_zyx) { |
| 172 | + double fuzz = use_zyx ? ZYX_Y_FUZZ : RPY_P_FUZZ; |
| 173 | + for (pole = -1; pole <= 1; pole += 2) |
| 174 | + for (i = 0; i < sizeof(pairs) / sizeof(pairs[0]); ++i) { |
| 175 | + double roll = pairs[i][0] * TO_RAD; |
| 176 | + double yaw = pairs[i][1] * TO_RAD; |
| 177 | + double canonical_roll = roll - pole * yaw; |
| 178 | + double s = sin(canonical_roll), c = cos(canonical_roll); |
| 179 | + /* Exact pole: zero entries must not come from cos(pi/2). */ |
| 180 | + matrix exact = {{0, pole * s, pole * c}, |
| 181 | + {0, c, -s}, {-pole, 0, 0}}; |
| 182 | + rtapi_snprintf(name, sizeof(name), |
| 183 | + "exact pole=%+d roll=%g yaw=%g", |
| 184 | + pole, pairs[i][0], pairs[i][1]); |
| 185 | + check_matrix(name, exact, pole, canonical_roll, TIGHT, |
| 186 | + 0, use_zyx, fuzz); |
| 187 | + |
| 188 | + for (sign = -1; sign <= 1; sign += 2) { |
| 189 | + rtapi_snprintf(name, sizeof(name), |
| 190 | + "quaternion sign=%+d pole=%+d pair=%u", |
| 191 | + sign, pole, i); |
| 192 | + check_quaternion(name, exact, pole, canonical_roll, |
| 193 | + sign, use_zyx, fuzz); |
| 194 | + } |
| 195 | + for (j = 0; j < sizeof(offsets) / sizeof(offsets[0]); ++j) |
| 196 | + for (side = -1; side <= 1; side += 2) { |
| 197 | + double delta = side * offsets[j] * fuzz; |
| 198 | + int inside = offsets[j] < 1; |
| 199 | + double tolerance = inside ? 2 * fuzz + TIGHT : TIGHT; |
| 200 | + rotation(roll, pole * PM_PI_2 + delta, yaw, input); |
| 201 | + rtapi_snprintf(name, sizeof(name), |
| 202 | + "near pole=%+d pair=%u delta=%+.2g*fuzz", |
| 203 | + pole, i, side * offsets[j]); |
| 204 | + check_matrix(name, input, inside ? pole : 0, |
| 205 | + canonical_roll, tolerance, !inside, |
| 206 | + use_zyx, fuzz); |
| 207 | + } |
| 208 | + } |
| 209 | + for (i = 0; i < sizeof(ordinary) / sizeof(ordinary[0]); ++i) { |
| 210 | + rotation(ordinary[i][0] * TO_RAD, ordinary[i][1] * TO_RAD, |
| 211 | + ordinary[i][2] * TO_RAD, input); |
| 212 | + rtapi_snprintf(name, sizeof(name), "ordinary rotation=%u", i); |
| 213 | + check_matrix(name, input, 0, 0, TIGHT, 0, use_zyx, fuzz); |
| 214 | + } |
| 215 | + } |
| 216 | + if (failures) { |
| 217 | + rtapi_print_msg(RTAPI_MSG_ERR, "eulercheck: %d check(s) failed\n", |
| 218 | + failures); |
| 219 | + hal_exit(comp_id); |
| 220 | + return -ERANGE; |
| 221 | + } |
| 222 | + hal_ready(comp_id); |
| 223 | + return 0; |
| 224 | +} |
| 225 | + |
| 226 | +void rtapi_app_exit(void) |
| 227 | +{ |
| 228 | + hal_exit(comp_id); |
| 229 | +} |
0 commit comments