franka/rate_limiting/
joint.rs

1//! Joint-space rate limiting: the position/velocity `limitRate` overloads and the deprecated
2//! FR3 position-dependent joint-velocity envelope.
3//!
4//! Split out of libfranka's `src/rate_limiting.cpp`; the constants these functions are normally
5//! called with live in [the parent module](super).
6
7use super::{DELTA_T, JOINT_VELOCITY_LIMITS_TOLERANCE};
8use crate::error::{FrankaError, FrankaResult};
9use crate::math_utils::{cmax, cmin};
10
11/// Computes the maximum joint velocity based on joint position.
12///
13/// Port of the (deprecated) `franka::computeUpperLimitsJointVelocity`, which hardcodes the FR3
14/// parameters. Prefer [`crate::joint_velocity_limits::JointVelocityLimitsConfig::upper_limits`],
15/// which reads the same parameters from the robot's URDF.
16///
17/// The hardcoded offsets and position limits are not those of Franka's FR3 specifications
18/// (e.g. joint 1: 0.30 and 2.7501 against 0.6599 and 2.9007); the port keeps libfranka's.
19pub fn compute_upper_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7] {
20    [
21        cmin(
22            2.62,
23            cmax(0.0, -0.30 + cmax(0.0, 12.0 * (2.75010 - q[0])).sqrt()),
24        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[0],
25        cmin(
26            2.62,
27            cmax(0.0, -0.20 + cmax(0.0, 5.17 * (1.79180 - q[1])).sqrt()),
28        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[1],
29        cmin(
30            2.62,
31            cmax(0.0, -0.20 + cmax(0.0, 7.00 * (2.90650 - q[2])).sqrt()),
32        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[2],
33        cmin(
34            2.62,
35            cmax(0.0, -0.30 + cmax(0.0, 8.00 * (-0.1458 - q[3])).sqrt()),
36        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[3],
37        cmin(
38            5.26,
39            cmax(0.0, -0.35 + cmax(0.0, 34.0 * (2.81010 - q[4])).sqrt()),
40        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[4],
41        cmin(
42            4.18,
43            cmax(0.0, -0.35 + cmax(0.0, 11.0 * (4.52050 - q[5])).sqrt()),
44        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[5],
45        cmin(
46            5.26,
47            cmax(0.0, -0.35 + cmax(0.0, 34.0 * (3.01960 - q[6])).sqrt()),
48        ) - JOINT_VELOCITY_LIMITS_TOLERANCE[6],
49    ]
50}
51
52/// Computes the minimum joint velocity based on joint position.
53///
54/// Port of the (deprecated) `franka::computeLowerLimitsJointVelocity`, which hardcodes the FR3
55/// parameters. Prefer [`crate::joint_velocity_limits::JointVelocityLimitsConfig::lower_limits`],
56/// which reads the same parameters from the robot's URDF. Its constants are libfranka's, as
57/// [`compute_upper_limits_joint_velocity`]'s.
58pub fn compute_lower_limits_joint_velocity(q: &[f64; 7]) -> [f64; 7] {
59    [
60        cmax(
61            -2.62,
62            cmin(0.0, 0.30 - cmax(0.0, 12.0 * (2.750100 + q[0])).sqrt()),
63        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[0],
64        cmax(
65            -2.62,
66            cmin(0.0, 0.20 - cmax(0.0, 5.17 * (1.791800 + q[1])).sqrt()),
67        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[1],
68        cmax(
69            -2.62,
70            cmin(0.0, 0.20 - cmax(0.0, 7.00 * (2.906500 + q[2])).sqrt()),
71        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[2],
72        cmax(
73            -2.62,
74            cmin(0.0, 0.30 - cmax(0.0, 8.00 * (3.048100 + q[3])).sqrt()),
75        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[3],
76        cmax(
77            -5.26,
78            cmin(0.0, 0.35 - cmax(0.0, 34.0 * (2.810100 + q[4])).sqrt()),
79        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[4],
80        cmax(
81            -4.18,
82            cmin(0.0, 0.35 - cmax(0.0, 11.0 * (-0.54092 + q[5])).sqrt()),
83        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[5],
84        cmax(
85            -5.26,
86            cmin(0.0, 0.35 - cmax(0.0, 34.0 * (3.019600 + q[6])).sqrt()),
87        ) + JOINT_VELOCITY_LIMITS_TOLERANCE[6],
88    ]
89}
90
91/// Limits the rate of a desired joint velocity considering the limits provided.
92///
93/// Port of the scalar `franka::limitRate(double upper_limits_velocity, ..., double
94/// commanded_velocity, ...)`.
95///
96/// Unlike the C++ overload, this function does not check `commanded_velocity` for finiteness
97/// (the fixed Rust interface makes it infallible); [`limit_rate_joint_velocities`] performs the
98/// check for the whole vector, exactly like the C++ vector overload does before delegating here.
99///
100/// # Note
101/// FCI filters must be deactivated to work properly.
102#[allow(clippy::too_many_arguments)]
103pub fn limit_rate_joint_velocity(
104    upper_limits_velocity: f64,
105    lower_limits_velocity: f64,
106    max_acceleration: f64,
107    max_jerk: f64,
108    commanded_velocity: f64,
109    last_commanded_velocity: f64,
110    last_commanded_acceleration: f64,
111) -> f64 {
112    // Differentiate to get jerk
113    let commanded_jerk = (((commanded_velocity - last_commanded_velocity) / DELTA_T)
114        - last_commanded_acceleration)
115        / DELTA_T;
116
117    // Limit jerk and integrate to get acceleration
118    let commanded_acceleration =
119        last_commanded_acceleration + cmax(cmin(commanded_jerk, max_jerk), -max_jerk) * DELTA_T;
120
121    // Compute acceleration limits
122    let safe_max_acceleration = cmin(
123        (max_jerk / max_acceleration) * (upper_limits_velocity - last_commanded_velocity),
124        max_acceleration,
125    );
126    let safe_min_acceleration = cmax(
127        (max_jerk / max_acceleration) * (lower_limits_velocity - last_commanded_velocity),
128        -max_acceleration,
129    );
130
131    // Limit acceleration and integrate to get desired velocities
132    last_commanded_velocity
133        + cmax(
134            cmin(commanded_acceleration, safe_max_acceleration),
135            safe_min_acceleration,
136        ) * DELTA_T
137}
138
139/// Limits the rate of a desired joint position considering the limits provided.
140///
141/// Port of the scalar `franka::limitRate(double upper_limits_velocity, ..., double
142/// commanded_position, ...)`.
143///
144/// Unlike the C++ overload, this function does not check `commanded_position` for finiteness
145/// (the fixed Rust interface makes it infallible); [`limit_rate_joint_positions`] performs the
146/// check for the whole vector, exactly like the C++ vector overload does before delegating here.
147///
148/// # Note
149/// FCI filters must be deactivated to work properly.
150#[allow(clippy::too_many_arguments)]
151pub fn limit_rate_joint_position(
152    upper_limits_velocity: f64,
153    lower_limits_velocity: f64,
154    max_acceleration: f64,
155    max_jerk: f64,
156    commanded_position: f64,
157    last_commanded_position: f64,
158    last_commanded_velocity: f64,
159    last_commanded_acceleration: f64,
160) -> f64 {
161    last_commanded_position
162        + limit_rate_joint_velocity(
163            upper_limits_velocity,
164            lower_limits_velocity,
165            max_acceleration,
166            max_jerk,
167            (commanded_position - last_commanded_position) / DELTA_T,
168            last_commanded_velocity,
169            last_commanded_acceleration,
170        ) * DELTA_T
171}
172
173/// Limits the rate of desired joint velocities considering the limits provided.
174///
175/// Port of the `std::array<double, 7>` velocity overload of `franka::limitRate`.
176///
177/// # Errors
178/// [`FrankaError::InvalidArgument`] if `commanded_velocities` are infinite or NaN.
179///
180/// # Note
181/// FCI filters must be deactivated to work properly.
182#[allow(clippy::too_many_arguments)]
183pub fn limit_rate_joint_velocities(
184    upper_limits_velocity: &[f64; 7],
185    lower_limits_velocity: &[f64; 7],
186    max_acceleration: &[f64; 7],
187    max_jerk: &[f64; 7],
188    commanded_velocities: &[f64; 7],
189    last_commanded_velocities: &[f64; 7],
190    last_commanded_accelerations: &[f64; 7],
191) -> FrankaResult<[f64; 7]> {
192    if !commanded_velocities.iter().all(|v| v.is_finite()) {
193        return Err(FrankaError::InvalidArgument(
194            "commanded_velocities is infinite or NaN.".to_string(),
195        ));
196    }
197    let mut limited = [0.0; 7];
198    for i in 0..7 {
199        limited[i] = limit_rate_joint_velocity(
200            upper_limits_velocity[i],
201            lower_limits_velocity[i],
202            max_acceleration[i],
203            max_jerk[i],
204            commanded_velocities[i],
205            last_commanded_velocities[i],
206            last_commanded_accelerations[i],
207        );
208    }
209    Ok(limited)
210}
211
212/// Limits the rate of desired joint positions considering the limits provided.
213///
214/// Port of the `std::array<double, 7>` position overload of `franka::limitRate`.
215///
216/// # Errors
217/// [`FrankaError::InvalidArgument`] if `commanded_positions` are infinite or NaN.
218///
219/// # Note
220/// FCI filters must be deactivated to work properly.
221#[allow(clippy::too_many_arguments)]
222pub fn limit_rate_joint_positions(
223    upper_limits_velocity: &[f64; 7],
224    lower_limits_velocity: &[f64; 7],
225    max_acceleration: &[f64; 7],
226    max_jerk: &[f64; 7],
227    commanded_positions: &[f64; 7],
228    last_commanded_positions: &[f64; 7],
229    last_commanded_velocities: &[f64; 7],
230    last_commanded_accelerations: &[f64; 7],
231) -> FrankaResult<[f64; 7]> {
232    if !commanded_positions.iter().all(|v| v.is_finite()) {
233        return Err(FrankaError::InvalidArgument(
234            "commanded_positions is infinite or NaN.".to_string(),
235        ));
236    }
237    let mut limited = [0.0; 7];
238    for i in 0..7 {
239        limited[i] = limit_rate_joint_position(
240            upper_limits_velocity[i],
241            lower_limits_velocity[i],
242            max_acceleration[i],
243            max_jerk[i],
244            commanded_positions[i],
245            last_commanded_positions[i],
246            last_commanded_velocities[i],
247            last_commanded_accelerations[i],
248        );
249    }
250    Ok(limited)
251}
franka/rate_limiting/joint.rs

franka/rate_limiting/
joint.rs
