Program Listing for File control_tools.h
↰ Return to documentation for file (include/franka/control_tools.h
)
// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <stdexcept>
#include <string>
namespace franka {
inline bool isValidElbow(const std::array<double, 2>& elbow) noexcept {
return elbow[1] == -1.0 || elbow[1] == 1.0;
}
inline bool isHomogeneousTransformation(const std::array<double, 16>& transform) noexcept {
constexpr double kOrthonormalThreshold = 1e-5;
if (transform[3] != 0.0 || transform[7] != 0.0 || transform[11] != 0.0 || transform[15] != 1.0) {
return false;
}
for (size_t j = 0; j < 3; ++j) { // i..column
if (std::abs(std::sqrt(std::pow(transform[j * 4 + 0], 2) + std::pow(transform[j * 4 + 1], 2) +
std::pow(transform[j * 4 + 2], 2)) -
1.0) > kOrthonormalThreshold) {
return false;
}
}
for (size_t i = 0; i < 3; ++i) { // j..row
if (std::abs(std::sqrt(std::pow(transform[0 * 4 + i], 2) + std::pow(transform[1 * 4 + i], 2) +
std::pow(transform[2 * 4 + i], 2)) -
1.0) > kOrthonormalThreshold) {
return false;
}
}
return true;
}
bool hasRealtimeKernel();
bool setCurrentThreadToHighestSchedulerPriority(std::string* error_message);
template <size_t N>
inline void checkFinite(const std::array<double, N>& array) {
if (!std::all_of(array.begin(), array.end(),
[](double array_element) { return std::isfinite(array_element); })) {
throw std::invalid_argument("Commanding value is infinite or NaN.");
}
}
inline void checkMatrix(const std::array<double, 16>& transform) {
checkFinite(transform);
if (!isHomogeneousTransformation(transform)) {
throw std::invalid_argument(
"libfranka: Attempt to set invalid transformation in motion generator. Has to be column "
"major!");
}
}
inline void checkElbow(const std::array<double, 2>& elbow) {
checkFinite(elbow);
if (!isValidElbow(elbow)) {
throw std::invalid_argument(
"Invalid elbow configuration given! Only +1 or -1 are allowed for the sign of the 4th "
"joint.");
}
}
} // namespace franka