Program Listing for File rate_limiting.h
↰ Return to documentation for file (include/franka/rate_limiting.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 <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