libfranka  0.14.1
FCI C++ API
control_tools.h
Go to the documentation of this file.
1 // Copyright (c) 2023 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <algorithm>
6 #include <array>
7 #include <cmath>
8 #include <stdexcept>
9 #include <string>
10 
16 namespace franka {
17 
25 inline bool isValidElbow(const std::array<double, 2>& elbow) noexcept {
26  return elbow[1] == -1.0 || elbow[1] == 1.0;
27 }
28 
36 inline bool isHomogeneousTransformation(const std::array<double, 16>& transform) noexcept {
37  constexpr double kOrthonormalThreshold = 1e-5;
38 
39  if (transform[3] != 0.0 || transform[7] != 0.0 || transform[11] != 0.0 || transform[15] != 1.0) {
40  return false;
41  }
42  for (size_t j = 0; j < 3; ++j) { // i..column
43  if (std::abs(std::sqrt(std::pow(transform[j * 4 + 0], 2) + std::pow(transform[j * 4 + 1], 2) +
44  std::pow(transform[j * 4 + 2], 2)) -
45  1.0) > kOrthonormalThreshold) {
46  return false;
47  }
48  }
49  for (size_t i = 0; i < 3; ++i) { // j..row
50  if (std::abs(std::sqrt(std::pow(transform[0 * 4 + i], 2) + std::pow(transform[1 * 4 + i], 2) +
51  std::pow(transform[2 * 4 + i], 2)) -
52  1.0) > kOrthonormalThreshold) {
53  return false;
54  }
55  }
56  return true;
57 }
58 
68 
77 bool setCurrentThreadToHighestSchedulerPriority(std::string* error_message);
78 
85 template <size_t N>
86 inline void checkFinite(const std::array<double, N>& array) {
87  if (!std::all_of(array.begin(), array.end(),
88  [](double array_element) { return std::isfinite(array_element); })) {
89  throw std::invalid_argument("Commanding value is infinite or NaN.");
90  }
91 }
92 
99 inline void checkMatrix(const std::array<double, 16>& transform) {
100  checkFinite(transform);
101  if (!isHomogeneousTransformation(transform)) {
102  throw std::invalid_argument(
103  "libfranka: Attempt to set invalid transformation in motion generator. Has to be column "
104  "major!");
105  }
106 }
107 
113 inline void checkElbow(const std::array<double, 2>& elbow) {
114  checkFinite(elbow);
115  if (!isValidElbow(elbow)) {
116  throw std::invalid_argument(
117  "Invalid elbow configuration given! Only +1 or -1 are allowed for the sign of the 4th "
118  "joint.");
119  }
120 }
121 
122 } // namespace franka
bool isValidElbow(const std::array< double, 2 > &elbow) noexcept
Determines whether the given elbow configuration is valid or not.
Definition: control_tools.h:25
bool setCurrentThreadToHighestSchedulerPriority(std::string *error_message)
Sets the current thread to the highest possible scheduler priority.
void checkFinite(const std::array< double, N > &array)
Checks if all elements of an array of the size N have a finite value.
Definition: control_tools.h:86
void checkMatrix(const std::array< double, 16 > &transform)
Checks if all elements of the transformation matrix are finite and if it is a homogeneous transformat...
Definition: control_tools.h:99
bool hasRealtimeKernel()
Determines whether the current OS kernel is a realtime kernel.
bool isHomogeneousTransformation(const std::array< double, 16 > &transform) noexcept
Determines whether the given array represents a valid homogeneous transformation matrix.
Definition: control_tools.h:36
void checkElbow(const std::array< double, 2 > &elbow)
Checks if all elements of the elbow vector are finite and if the elbow configuration is valid.
Definition: control_tools.h:113