.. _program_listing_file_include_franka_control_tools.h:

Program Listing for File control_tools.h
========================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_franka_control_tools.h>` (``include/franka/control_tools.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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