23 inline bool isValidElbow(
const std::array<double, 2>& elbow) noexcept {
24 return elbow[1] == -1.0 || elbow[1] == 1.0;
35 constexpr
double kOrthonormalThreshold = 1e-5;
37 if (transform[3] != 0.0 || transform[7] != 0.0 || transform[11] != 0.0 || transform[15] != 1.0) {
40 for (
size_t j = 0; j < 3; ++j) {
41 if (std::abs(std::sqrt(std::pow(transform[j * 4 + 0], 2) + std::pow(transform[j * 4 + 1], 2) +
42 std::pow(transform[j * 4 + 2], 2)) -
43 1.0) > kOrthonormalThreshold) {
47 for (
size_t i = 0; i < 3; ++i) {
48 if (std::abs(std::sqrt(std::pow(transform[0 * 4 + i], 2) + std::pow(transform[1 * 4 + i], 2) +
49 std::pow(transform[2 * 4 + i], 2)) -
50 1.0) > kOrthonormalThreshold) {
Definition: command_types.h:13