libfranka  0.14.1
FCI C++ API
rate_limiting.h
Go to the documentation of this file.
1 // Copyright (c) 2023 Franka Robotics GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <algorithm>
6 #include <array>
7 #include <cmath>
8 #include <limits>
9 
16 namespace franka {
20 constexpr double kDeltaT = 1e-3;
24 constexpr double kLimitEps = 1e-3;
28 constexpr double kNormEps = std::numeric_limits<double>::epsilon();
35 constexpr double kTolNumberPacketsLost = 0.0;
39 constexpr double kFactorCartesianRotationPoseInterface = 0.99;
43 constexpr std::array<double, 7> kMaxTorqueRate{
44  {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps,
45  1000 - kLimitEps, 1000 - kLimitEps}};
49 constexpr std::array<double, 7> kMaxJointJerk{
50  {5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps,
51  5000.0 - kLimitEps, 5000.0 - kLimitEps, 5000.0 - kLimitEps}};
55 constexpr std::array<double, 7> kMaxJointAcceleration{
56  {10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps,
57  10.0000 - kLimitEps, 10.0000 - kLimitEps, 10.0000 - kLimitEps}};
61 constexpr std::array<double, 7> kJointVelocityLimitsTolerance{
69 };
73 constexpr double kMaxTranslationalJerk = 4500.0 - kLimitEps;
77 constexpr double kMaxTranslationalAcceleration = 9.0000 - kLimitEps;
81 constexpr double kMaxTranslationalVelocity =
86 constexpr double kMaxRotationalJerk = 8500.0 - kLimitEps;
90 constexpr double kMaxRotationalAcceleration = 17.0000 - kLimitEps;
94 constexpr double kMaxRotationalVelocity =
99 constexpr double kMaxElbowJerk = 5000 - kLimitEps;
103 constexpr double kMaxElbowAcceleration = 10.0000 - kLimitEps;
107 constexpr double kMaxElbowVelocity =
109 
120 inline std::array<double, 7> computeUpperLimitsJointVelocity(const std::array<double, 7>& q) {
121  return std::array<double, 7>{
122  std::min(2.62, std::max(0.0, -0.30 + std::sqrt(std::max(0.0, 12.0 * (2.75010 - q[0]))))) -
124  std::min(2.62, std::max(0.0, -0.20 + std::sqrt(std::max(0.0, 5.17 * (1.79180 - q[1]))))) -
126  std::min(2.62, std::max(0.0, -0.20 + std::sqrt(std::max(0.0, 7.00 * (2.90650 - q[2]))))) -
128  std::min(2.62, std::max(0.0, -0.30 + std::sqrt(std::max(0.0, 8.00 * (-0.1458 - q[3]))))) -
130  std::min(5.26, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 34.0 * (2.81010 - q[4]))))) -
132  std::min(4.18, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 11.0 * (4.52050 - q[5]))))) -
134  std::min(5.26, std::max(0.0, -0.35 + std::sqrt(std::max(0.0, 34.0 * (3.01960 - q[6]))))) -
136  };
137 }
138 
149 inline std::array<double, 7> computeLowerLimitsJointVelocity(const std::array<double, 7>& q) {
150  return std::array<double, 7>{
151  std::max(-2.62, std::min(0.0, 0.30 - std::sqrt(std::max(0.0, 12.0 * (2.750100 + q[0]))))) +
153  std::max(-2.62, std::min(0.0, 0.20 - std::sqrt(std::max(0.0, 5.17 * (1.791800 + q[1]))))) +
155  std::max(-2.62, std::min(0.0, 0.20 - std::sqrt(std::max(0.0, 7.00 * (2.906500 + q[2]))))) +
157  std::max(-2.62, std::min(0.0, 0.30 - std::sqrt(std::max(0.0, 8.00 * (3.048100 + q[3]))))) +
159  std::max(-5.26, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 34.0 * (2.810100 + q[4]))))) +
161  std::max(-4.18, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 11.0 * (-0.54092 + q[5]))))) +
163  std::max(-5.26, std::min(0.0, 0.35 - std::sqrt(std::max(0.0, 34.0 * (3.019600 + q[6]))))) +
165  };
166 }
167 
183 std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
184  const std::array<double, 7>& commanded_values,
185  const std::array<double, 7>& last_commanded_values);
186 
205 double limitRate(double upper_limits_velocity,
206  double lower_limits_velocity,
207  double max_acceleration,
208  double max_jerk,
209  double commanded_velocity,
210  double last_commanded_velocity,
211  double last_commanded_acceleration);
212 
232 double limitRate(double upper_limits_velocity,
233  double lower_limits_velocity,
234  double max_acceleration,
235  double max_jerk,
236  double commanded_position,
237  double last_commanded_position,
238  double last_commanded_velocity,
239  double last_commanded_acceleration);
240 
259 std::array<double, 7> limitRate(const std::array<double, 7>& upper_limits_velocity,
260  const std::array<double, 7>& lower_limits_velocity,
261  const std::array<double, 7>& max_acceleration,
262  const std::array<double, 7>& max_jerk,
263  const std::array<double, 7>& commanded_velocities,
264  const std::array<double, 7>& last_commanded_velocities,
265  const std::array<double, 7>& last_commanded_accelerations);
266 
286 std::array<double, 7> limitRate(const std::array<double, 7>& upper_limits_velocity,
287  const std::array<double, 7>& lower_limits_velocity,
288  const std::array<double, 7>& max_acceleration,
289  const std::array<double, 7>& max_jerk,
290  const std::array<double, 7>& commanded_positions,
291  const std::array<double, 7>& last_commanded_positions,
292  const std::array<double, 7>& last_commanded_velocities,
293  const std::array<double, 7>& last_commanded_accelerations);
294 
315 std::array<double, 6> limitRate(
316  double max_translational_velocity,
317  double max_translational_acceleration,
318  double max_translational_jerk,
319  double max_rotational_velocity,
320  double max_rotational_acceleration,
321  double max_rotational_jerk,
322  const std::array<double, 6>& O_dP_EE_c, // NOLINT(readability-identifier-naming)
323  const std::array<double, 6>& last_O_dP_EE_c, // NOLINT(readability-identifier-naming)
324  const std::array<double, 6>& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming)
325 
347 std::array<double, 16> limitRate(
348  double max_translational_velocity,
349  double max_translational_acceleration,
350  double max_translational_jerk,
351  double max_rotational_velocity,
352  double max_rotational_acceleration,
353  double max_rotational_jerk,
354  const std::array<double, 16>& O_T_EE_c, // NOLINT(readability-identifier-naming)
355  const std::array<double, 16>& last_O_T_EE_c, // NOLINT(readability-identifier-naming)
356  const std::array<double, 6>& last_O_dP_EE_c, // NOLINT(readability-identifier-naming)
357  const std::array<double, 6>& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming)
358 
359 } // namespace franka
constexpr double kFactorCartesianRotationPoseInterface
Factor for the definition of rotational limits using the Cartesian Pose interface.
Definition: rate_limiting.h:39
constexpr double kDeltaT
Sample time constant.
Definition: rate_limiting.h:20
constexpr double kMaxRotationalJerk
Maximum rotational jerk.
Definition: rate_limiting.h:86
constexpr double kMaxElbowVelocity
Maximum elbow velocity.
Definition: rate_limiting.h:107
constexpr double kMaxTranslationalAcceleration
Maximum translational acceleration.
Definition: rate_limiting.h:77
constexpr std::array< double, 7 > kJointVelocityLimitsTolerance
Tolerance value for joint velocity limits to deal with numerical errors and data losses.
Definition: rate_limiting.h:61
std::array< double, 7 > computeUpperLimitsJointVelocity(const std::array< double, 7 > &q)
Computes the maximum joint velocity based on joint position.
Definition: rate_limiting.h:120
constexpr double kNormEps
Epsilon value for limiting Cartesian accelerations/jerks or not.
Definition: rate_limiting.h:28
constexpr double kMaxTranslationalJerk
Maximum translational jerk.
Definition: rate_limiting.h:73
constexpr double kMaxRotationalAcceleration
Maximum rotational acceleration.
Definition: rate_limiting.h:90
constexpr std::array< double, 7 > kMaxJointJerk
Maximum joint jerk.
Definition: rate_limiting.h:49
constexpr double kTolNumberPacketsLost
Number of packets lost considered for the definition of velocity limits.
Definition: rate_limiting.h:35
constexpr std::array< double, 7 > kMaxTorqueRate
Maximum torque rate.
Definition: rate_limiting.h:43
std::array< double, 7 > computeLowerLimitsJointVelocity(const std::array< double, 7 > &q)
Computes the minimum joint velocity based on joint position.
Definition: rate_limiting.h:149
constexpr std::array< double, 7 > kMaxJointAcceleration
Maximum joint acceleration.
Definition: rate_limiting.h:55
constexpr double kMaxTranslationalVelocity
Maximum translational velocity.
Definition: rate_limiting.h:81
constexpr double kLimitEps
Epsilon value for checking limits.
Definition: rate_limiting.h:24
constexpr double kMaxRotationalVelocity
Maximum rotational velocity.
Definition: rate_limiting.h:94
constexpr double kMaxElbowJerk
Maximum elbow jerk.
Definition: rate_limiting.h:99
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)
Limits the rate of an input vector of per-joint commands considering the maximum allowed time derivat...
constexpr double kMaxElbowAcceleration
Maximum elbow acceleration.
Definition: rate_limiting.h:103