rate_limiting.h
Go to the documentation of this file.
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #pragma once
4 
5 #include <array>
6 #include <limits>
7 
14 namespace franka {
18 constexpr double kDeltaT = 1e-3;
22 constexpr double kLimitEps = 1e-3;
26 constexpr double kNormEps = std::numeric_limits<double>::epsilon();
31 constexpr double kTolNumberPacketsLost = 3.0;
35 constexpr double kFactorCartesianRotationPoseInterface = 0.99;
39 constexpr std::array<double, 7> kMaxTorqueRate{
40  {1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps, 1000 - kLimitEps,
41  1000 - kLimitEps, 1000 - kLimitEps}};
45 constexpr std::array<double, 7> kMaxJointJerk{
46  {7500.0 - kLimitEps, 3750.0 - kLimitEps, 5000.0 - kLimitEps, 6250.0 - kLimitEps,
47  7500.0 - kLimitEps, 10000.0 - kLimitEps, 10000.0 - kLimitEps}};
51 constexpr std::array<double, 7> kMaxJointAcceleration{
52  {15.0000 - kLimitEps, 7.500 - kLimitEps, 10.0000 - kLimitEps, 12.5000 - kLimitEps,
53  15.0000 - kLimitEps, 20.0000 - kLimitEps, 20.0000 - kLimitEps}};
57 constexpr std::array<double, 7> kMaxJointVelocity{
58  {2.1750 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxJointAcceleration[0],
59  2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[1],
60  2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[2],
61  2.1750 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[3],
62  2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[4],
63  2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[5],
64  2.6100 - kLimitEps - kTolNumberPacketsLost* kDeltaT* kMaxJointAcceleration[6]}};
68 constexpr double kMaxTranslationalJerk = 6500.0 - kLimitEps;
72 constexpr double kMaxTranslationalAcceleration = 13.0000 - kLimitEps;
76 constexpr double kMaxTranslationalVelocity =
77  1.7000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxTranslationalAcceleration;
81 constexpr double kMaxRotationalJerk = 12500.0 - kLimitEps;
85 constexpr double kMaxRotationalAcceleration = 25.0000 - kLimitEps;
89 constexpr double kMaxRotationalVelocity =
90  2.5000 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxRotationalAcceleration;
94 constexpr double kMaxElbowJerk = 5000 - kLimitEps;
98 constexpr double kMaxElbowAcceleration = 10.0000 - kLimitEps;
102 constexpr double kMaxElbowVelocity =
103  2.1750 - kLimitEps - kTolNumberPacketsLost * kDeltaT * kMaxElbowAcceleration;
104 
120 std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
121  const std::array<double, 7>& commanded_values,
122  const std::array<double, 7>& last_commanded_values);
123 
141 double limitRate(double max_velocity,
142  double max_acceleration,
143  double max_jerk,
144  double commanded_velocity,
145  double last_commanded_velocity,
146  double last_commanded_acceleration);
147 
166 double limitRate(double max_velocity,
167  double max_acceleration,
168  double max_jerk,
169  double commanded_position,
170  double last_commanded_position,
171  double last_commanded_velocity,
172  double last_commanded_acceleration);
173 
191 std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
192  const std::array<double, 7>& max_acceleration,
193  const std::array<double, 7>& max_jerk,
194  const std::array<double, 7>& commanded_velocities,
195  const std::array<double, 7>& last_commanded_velocities,
196  const std::array<double, 7>& last_commanded_accelerations);
197 
216 std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
217  const std::array<double, 7>& max_acceleration,
218  const std::array<double, 7>& max_jerk,
219  const std::array<double, 7>& commanded_positions,
220  const std::array<double, 7>& last_commanded_positions,
221  const std::array<double, 7>& last_commanded_velocities,
222  const std::array<double, 7>& last_commanded_accelerations);
223 
244 std::array<double, 6> limitRate(
245  double max_translational_velocity,
246  double max_translational_acceleration,
247  double max_translational_jerk,
248  double max_rotational_velocity,
249  double max_rotational_acceleration,
250  double max_rotational_jerk,
251  const std::array<double, 6>& O_dP_EE_c, // NOLINT(readability-identifier-naming)
252  const std::array<double, 6>& last_O_dP_EE_c, // NOLINT(readability-identifier-naming)
253  const std::array<double, 6>& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming)
254 
276 std::array<double, 16> limitRate(
277  double max_translational_velocity,
278  double max_translational_acceleration,
279  double max_translational_jerk,
280  double max_rotational_velocity,
281  double max_rotational_acceleration,
282  double max_rotational_jerk,
283  const std::array<double, 16>& O_T_EE_c, // NOLINT(readability-identifier-naming)
284  const std::array<double, 16>& last_O_T_EE_c, // NOLINT(readability-identifier-naming)
285  const std::array<double, 6>& last_O_dP_EE_c, // NOLINT(readability-identifier-naming)
286  const std::array<double, 6>& last_O_ddP_EE_c); // NOLINT(readability-identifier-naming)
287 
288 } // namespace franka
constexpr double kMaxElbowJerk
Maximum elbow jerk.
Definition: rate_limiting.h:94
constexpr std::array< double, 7 > kMaxJointVelocity
Maximum joint velocity.
Definition: rate_limiting.h:57
constexpr double kMaxTranslationalVelocity
Maximum translational velocity.
Definition: rate_limiting.h:76
constexpr double kMaxTranslationalAcceleration
Maximum translational acceleration.
Definition: rate_limiting.h:72
constexpr double kNormEps
Epsilon value for limiting Cartesian accelerations/jerks or not.
Definition: rate_limiting.h:26
constexpr std::array< double, 7 > kMaxTorqueRate
Maximum torque rate.
Definition: rate_limiting.h:39
constexpr std::array< double, 7 > kMaxJointJerk
Maximum joint jerk.
Definition: rate_limiting.h:45
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:98
constexpr double kFactorCartesianRotationPoseInterface
Factor for the definition of rotational limits using the Cartesian Pose interface.
Definition: rate_limiting.h:35
constexpr double kTolNumberPacketsLost
Number of packets losts considered for the definition of velocity limits.
Definition: rate_limiting.h:31
constexpr double kLimitEps
Epsilon value for checking limits.
Definition: rate_limiting.h:22
constexpr double kMaxTranslationalJerk
Maximum translational jerk.
Definition: rate_limiting.h:68
constexpr double kMaxRotationalJerk
Maximum rotational jerk.
Definition: rate_limiting.h:81
constexpr double kMaxElbowVelocity
Maximum elbow velocity.
constexpr double kMaxRotationalAcceleration
Maximum rotational acceleration.
Definition: rate_limiting.h:85
constexpr std::array< double, 7 > kMaxJointAcceleration
Maximum joint acceleration.
Definition: rate_limiting.h:51
constexpr double kMaxRotationalVelocity
Maximum rotational velocity.
Definition: rate_limiting.h:89
constexpr double kDeltaT
Sample time constant.
Definition: rate_limiting.h:18


libfranka
Author(s): Franka Emika GmbH
autogenerated on Tue Jul 9 2019 03:32:01