rate_limiting.h

Go to the documentation of this file.

// 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 <limits>
 
namespace franka {
constexpr double kDeltaT = 1e-3;
constexpr double kLimitEps = 1e-3;
constexpr double kNormEps = std::numeric_limits<double>::epsilon();
constexpr double kTolNumberPacketsLost = 0.0;
constexpr double kFactorCartesianRotationPoseInterface = 0.99;
constexpr std::array<double, 7> kMaxTorqueRate{
    {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps,
     1000 - kLimitEps, 1000 - kLimitEps}};
constexpr std::array<double, 7> kMaxJointJerk{
    {5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps,
     5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps}};
constexpr std::array<double, 7> kMaxJointAcceleration{
    {10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps,
     10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps}};
constexpr std::array<double, 7> kJointVelocityLimitsTolerance{
    kLimitEps + kTolNumberPacketsLost * kDeltaT * kMaxJointAcceleration[0],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[1],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[2],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[3],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[4],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[5],
    kLimitEps + kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[6],
};
constexpr double kMaxTranslationalJerk = 4500.0 - kLimitEps;
constexpr double kMaxTranslationalAcceleration = 9.0000 - kLimitEps;
constexpr double kMaxTranslationalVelocity =
    3.0000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxTranslationalAcceleration;
constexpr double kMaxRotationalJerk = 8500.0 - kLimitEps;
constexpr double kMaxRotationalAcceleration = 17.0000 - kLimitEps;
constexpr double kMaxRotationalVelocity =
    2.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxRotationalAcceleration;
constexpr double kMaxElbowJerk = 5000 - kLimitEps;
constexpr double kMaxElbowAcceleration = 10.0000 - kLimitEps;
constexpr double kMaxElbowVelocity =
    1.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxElbowAcceleration;
 
inline std::array<double, 7> computeUpperLimitsJointVelocity(const std::array<double, 7>& q) {
  return std::array<double, 7>{
      std::min(2.62, std::max(0.0, -0.30 + std::sqrt(std::max(0.0, 12.0 * (2.75010 - q[0]))))) -
          kJointVelocityLimitsTolerance[0],
      std::min(2.62, std::max(0.0, -0.20 + std::sqrt(std::max(0.0, 5.17 * (1.79180 - q[1]))))) -
          kJointVelocityLimitsTolerance[1],
      std::min(2.62, std::max(0.0, -0.20 + std::sqrt(std::max(0.0, 7.00 * (2.90650 - q[2]))))) -
          kJointVelocityLimitsTolerance[2],
      std::min(2.62, std::max(0.0, -0.30 + std::sqrt(std::max(0.0, 8.00 * (-0.1458 - q[3]))))) -
          kJointVelocityLimitsTolerance[3],
      std::min(5.26, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 34.0 * (2.81010 - q[4]))))) -
          kJointVelocityLimitsTolerance[4],
      std::min(4.18, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 11.0 * (4.52050 - q[5]))))) -
          kJointVelocityLimitsTolerance[5],
      std::min(5.26, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 34.0 * (3.01960 - q[6]))))) -
          kJointVelocityLimitsTolerance[6],
  };
}
 
inline std::array<double, 7> computeLowerLimitsJointVelocity(const std::array<double, 7>& q) {
  return std::array<double, 7>{
      std::max(-2.62, std::min(0.0, 0.30 - std::sqrt(std::max(0.0, 12.0 * (2.750100 + q[0]))))) +
          kJointVelocityLimitsTolerance[0],
      std::max(-2.62, std::min(0.0, 0.20 - std::sqrt(std::max(0.0, 5.17 * (1.791800 + q[1]))))) +
          kJointVelocityLimitsTolerance[1],
      std::max(-2.62, std::min(0.0, 0.20 - std::sqrt(std::max(0.0, 7.00 * (2.906500 + q[2]))))) +
          kJointVelocityLimitsTolerance[2],
      std::max(-2.62, std::min(0.0, 0.30 - std::sqrt(std::max(0.0, 8.00 * (3.048100 + q[3]))))) +
          kJointVelocityLimitsTolerance[3],
      std::max(-5.26, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 34.0 * (2.810100 + q[4]))))) +
          kJointVelocityLimitsTolerance[4],
      std::max(-4.18, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 11.0 * (-0.54092 + q[5]))))) +
          kJointVelocityLimitsTolerance[5],
      std::max(-5.26, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 34.0 * (3.019600 + q[6]))))) +
          kJointVelocityLimitsTolerance[6],
  };
}
 
std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
                                const std::array<double, 7>& commanded_values,
                                const std::array<double, 7>& last_commanded_values);
 
double limitRate(double upper_limits_velocity,
                 double lower_limits_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_velocity,
                 double last_commanded_velocity,
                 double last_commanded_acceleration);
 
double limitRate(double upper_limits_velocity,
                 double lower_limits_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_position,
                 double last_commanded_position,
                 double last_commanded_velocity,
                 double last_commanded_acceleration);
 
std::array<double, 7> limitRate(const std::array<double, 7>& upper_limits_velocity,
                                const std::array<double, 7>& lower_limits_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_velocities,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations);
 
std::array<double, 7> limitRate(const std::array<double, 7>& upper_limits_velocity,
                                const std::array<double, 7>& lower_limits_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_positions,
                                const std::array<double, 7>& last_commanded_positions,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations);
 
std::array<double, 6> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 6>& O_dP_EE_c,         // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c);  // NOLINT(readability-identifier-naming)
 
std::array<double, 16> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 16>& O_T_EE_c,         // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& last_O_T_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,    // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c);  // NOLINT(readability-identifier-naming)
 
}  // namespace franka