franka/rate_limiting/
torque.rs

1//! Torque rate limiting: the `max_derivatives` overload of `franka::limitRate`.
2//!
3//! Split out of libfranka's `src/rate_limiting.cpp`; the constants this function is normally
4//! called with live in [the parent module](super).
5
6use super::DELTA_T;
7use crate::error::{FrankaError, FrankaResult};
8use crate::math_utils::{cmax, cmin};
9
10/// Limits the rate of an input vector of per-joint commands considering the maximum allowed
11/// time derivatives.
12///
13/// Port of `franka::limitRate(const std::array<double, 7>& max_derivatives, ...)`, used for
14/// torque commands.
15///
16/// # Errors
17/// [`FrankaError::InvalidArgument`] if `commanded_values` are infinite or NaN.
18///
19/// # Note
20/// FCI filters must be deactivated to work properly.
21///
22/// # float32 quantisation
23/// `last_commanded_values` is normally `RobotState::tau_J_d`, which the robot publishes as a
24/// `float32` while judging our command's rate against the `double` it last received. Saturating
25/// this function against the quantised value can therefore read as up to ≈ 1000.0028 Nm/s at
26/// the robot for |τ| ≥ 64 Nm — libfranka has the identical exposure, and this function stays an
27/// exact port of `franka::limitRate`. The control loop compensates by shrinking
28/// `max_derivatives` per cycle; see `crate::robot::control_loop::torque::torque_rate_margin`.
29pub fn limit_rate_torques(
30    max_derivatives: &[f64; 7],
31    commanded_values: &[f64; 7],
32    last_commanded_values: &[f64; 7],
33) -> FrankaResult<[f64; 7]> {
34    if !commanded_values.iter().all(|v| v.is_finite()) {
35        return Err(FrankaError::InvalidArgument(
36            "Commanding value is infinite or NaN.".to_string(),
37        ));
38    }
39    let mut limited_values = [0.0; 7];
40    for i in 0..7 {
41        let commanded_derivative = (commanded_values[i] - last_commanded_values[i]) / DELTA_T;
42        limited_values[i] = last_commanded_values[i]
43            + cmax(
44                cmin(commanded_derivative, max_derivatives[i]),
45                -max_derivatives[i],
46            ) * DELTA_T;
47    }
48    Ok(limited_values)
49}
franka/rate_limiting/torque.rs

franka/rate_limiting/
torque.rs
