.. _program_listing_file_include_franka_rate_limiting.h: Program Listing for File rate_limiting.h ======================================== |exhale_lsh| :ref:`Return to documentation for file ` (``include/franka/rate_limiting.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 #include #include #include namespace franka { constexpr double kDeltaT = 1e-3; constexpr double kLimitEps = 1e-3; constexpr double kNormEps = std::numeric_limits::epsilon(); constexpr double kTolNumberPacketsLost = 0.0; constexpr double kFactorCartesianRotationPoseInterface = 0.99; constexpr std::array kMaxTorqueRate{ {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps}}; constexpr std::array 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 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 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 computeUpperLimitsJointVelocity(const std::array& q) { return std::array{ 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 computeLowerLimitsJointVelocity(const std::array& q) { return std::array{ 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 limitRate(const std::array& max_derivatives, const std::array& commanded_values, const std::array& 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 limitRate(const std::array& upper_limits_velocity, const std::array& lower_limits_velocity, const std::array& max_acceleration, const std::array& max_jerk, const std::array& commanded_velocities, const std::array& last_commanded_velocities, const std::array& last_commanded_accelerations); std::array limitRate(const std::array& upper_limits_velocity, const std::array& lower_limits_velocity, const std::array& max_acceleration, const std::array& max_jerk, const std::array& commanded_positions, const std::array& last_commanded_positions, const std::array& last_commanded_velocities, const std::array& last_commanded_accelerations); std::array 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& O_dP_EE_c, // NOLINT(readability-identifier-naming) const std::array& last_O_dP_EE_c, // NOLINT(readability-identifier-naming) const std::array& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming) std::array 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& O_T_EE_c, // NOLINT(readability-identifier-naming) const std::array& last_O_T_EE_c, // NOLINT(readability-identifier-naming) const std::array& last_O_dP_EE_c, // NOLINT(readability-identifier-naming) const std::array& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming) } // namespace franka