franka/rate_limiting/
mod.rs

1//! Rate limiting for torques, joint positions/velocities and Cartesian poses/velocities.
2//!
3//! Port of libfranka 0.21.2 `include/franka/rate_limiting.h` and `src/rate_limiting.cpp`.
4//! The C++ overload set `franka::limitRate` is spelled out here as separate functions, since
5//! Rust has no overloading:
6//!
7//! | C++ | Rust |
8//! |---|---|
9//! | `limitRate(max_derivatives, commanded, last)` | [`limit_rate_torques`] |
10//! | `limitRate(upper, lower, acc, jerk, v, v_last, a_last)` | [`limit_rate_joint_velocity`] |
11//! | `limitRate(upper, lower, acc, jerk, q, q_last, v_last, a_last)` | [`limit_rate_joint_position`] |
12//! | the `array<double, 7>` variants of the two above | [`limit_rate_joint_velocities`], [`limit_rate_joint_positions`] |
13//! | `limitRate(..., O_dP_EE_c, ...)` | [`limit_rate_cartesian_velocity`] |
14//! | `limitRate(..., O_T_EE_c, ...)` | [`limit_rate_cartesian_pose`] |
15//!
16//! Where the C++ throws `std::invalid_argument`, these functions return
17//! [`crate::error::FrankaError::InvalidArgument`] with the same message text.
18//!
19//! The implementation is split by the quantity being limited (`torque`, `joint`, `cartesian`),
20//! but those modules are private: every function is re-exported here, so the public paths are
21//! `franka::rate_limiting::limit_rate_*` and nothing else.
22
23mod cartesian;
24pub mod fer;
25mod joint;
26mod torque;
27
28pub(crate) use cartesian::scaled_axis;
29pub use cartesian::{limit_rate_cartesian_pose, limit_rate_cartesian_velocity};
30pub use joint::{
31    compute_lower_limits_joint_velocity, compute_upper_limits_joint_velocity,
32    limit_rate_joint_position, limit_rate_joint_positions, limit_rate_joint_velocities,
33    limit_rate_joint_velocity,
34};
35pub use torque::limit_rate_torques;
36
37/// Sample time constant. Port of `franka::kDeltaT`.
38pub const DELTA_T: f64 = 1e-3;
39/// Epsilon value for checking limits. Port of `franka::kLimitEps`.
40pub const LIMIT_EPS: f64 = 1e-3;
41/// Epsilon value for limiting Cartesian accelerations/jerks or not. Port of `franka::kNormEps`.
42pub const NORM_EPS: f64 = f64::EPSILON;
43/// Number of packets lost considered for the definition of velocity limits.
44///
45/// When a packet is lost, FCI assumes a constant acceleration model. For FR3 there are no
46/// expected packet losses, therefore this number is 0. Port of `franka::kTolNumberPacketsLost`.
47pub const TOL_NUMBER_PACKETS_LOST: f64 = 0.0;
48/// Factor for the definition of rotational limits using the Cartesian pose interface.
49///
50/// Port of `franka::kFactorCartesianRotationPoseInterface`.
51pub const FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE: f64 = 0.99;
52
53/// Maximum torque rate. Port of `franka::kMaxTorqueRate`.
54///
55/// The `LIMIT_EPS` margin below the nominal 1000 Nm/s is *not* enough to absorb the `float32`
56/// quantisation of `tau_J_d` at |τ| ≥ 64 Nm; see [`limit_rate_torques`].
57pub const MAX_TORQUE_RATE: [f64; 7] = [1000.0 - LIMIT_EPS; 7];
58/// Maximum joint jerk. Port of `franka::kMaxJointJerk`.
59pub const MAX_JOINT_JERK: [f64; 7] = [5000.0 - LIMIT_EPS; 7];
60/// Maximum joint acceleration. Port of `franka::kMaxJointAcceleration`.
61pub const MAX_JOINT_ACCELERATION: [f64; 7] = [10.0 - LIMIT_EPS; 7];
62/// Tolerance value for joint velocity limits to deal with numerical errors and data losses.
63///
64/// Port of `franka::kJointVelocityLimitsTolerance`.
65pub const JOINT_VELOCITY_LIMITS_TOLERANCE: [f64; 7] = [
66    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[0],
67    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[1],
68    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[2],
69    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[3],
70    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[4],
71    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[5],
72    LIMIT_EPS + TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[6],
73];
74/// Maximum translational jerk. Port of `franka::kMaxTranslationalJerk`.
75pub const MAX_TRANSLATIONAL_JERK: f64 = 4500.0 - LIMIT_EPS;
76/// Maximum translational acceleration. Port of `franka::kMaxTranslationalAcceleration`.
77pub const MAX_TRANSLATIONAL_ACCELERATION: f64 = 9.0 - LIMIT_EPS;
78/// Maximum translational velocity. Port of `franka::kMaxTranslationalVelocity`.
79pub const MAX_TRANSLATIONAL_VELOCITY: f64 =
80    3.0 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_TRANSLATIONAL_ACCELERATION;
81/// Maximum rotational jerk. Port of `franka::kMaxRotationalJerk`.
82pub const MAX_ROTATIONAL_JERK: f64 = 8500.0 - LIMIT_EPS;
83/// Maximum rotational acceleration. Port of `franka::kMaxRotationalAcceleration`.
84pub const MAX_ROTATIONAL_ACCELERATION: f64 = 17.0 - LIMIT_EPS;
85/// Maximum rotational velocity. Port of `franka::kMaxRotationalVelocity`.
86pub const MAX_ROTATIONAL_VELOCITY: f64 =
87    2.5 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ROTATIONAL_ACCELERATION;
88/// Maximum elbow jerk. Port of `franka::kMaxElbowJerk`.
89pub const MAX_ELBOW_JERK: f64 = 5000.0 - LIMIT_EPS;
90/// Maximum elbow acceleration. Port of `franka::kMaxElbowAcceleration`.
91pub const MAX_ELBOW_ACCELERATION: f64 = 10.0 - LIMIT_EPS;
92/// Maximum elbow velocity. Port of `franka::kMaxElbowVelocity`.
93pub const MAX_ELBOW_VELOCITY: f64 =
94    1.5 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ELBOW_ACCELERATION;
95/// Minimum elbow velocity. Port of `franka::kMinElbowVelocity`.
96pub const MIN_ELBOW_VELOCITY: f64 = -MAX_ELBOW_VELOCITY;
97/// Joint position limits (lower, upper), rad: the `<limit>` of `joint1..7` in the FR3 URDF of
98/// libfranka's test suite (`test/fr3.urdf`, the crate's `tests/data/fr3.urdf`).
99pub const JOINT_POSITION_LIMITS: ([f64; 7], [f64; 7]) = (
100    [
101        -2.7501, -1.7918, -2.9065, -3.0481, -2.8101, 0.54092, -3.0196,
102    ],
103    [2.7501, 1.7918, 2.9065, -0.1458, 2.8101, 4.5205, 3.0196],
104);
105
106#[cfg(test)]
107mod tests;
franka/rate_limiting/mod.rs

franka/rate_limiting/
mod.rs
