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

franka/rate_limiting/
joint.rs
