franka/rate_limiting/
cartesian.rs

1//! Cartesian rate limiting: the `O_dP_EE_c` (twist) and `O_T_EE_c` (pose) `limitRate` overloads
2//! and the `Eigen::Vector3d` helper they share.
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 nalgebra::{Matrix3, Matrix4, Rotation3, UnitQuaternion, Vector3};
8
9use super::{DELTA_T, FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE, NORM_EPS};
10use crate::control_types::is_homogeneous_transformation;
11use crate::error::{FrankaError, FrankaResult};
12use crate::math_utils::{
13    cmax, cmin, linear_of, orthonormalized_rotation, pose_to_array, translation_of,
14};
15
16/// Limits the rate of a Cartesian (translational or rotational) velocity vector.
17///
18/// Port of the anonymous-namespace `limitRate(..., const Eigen::Vector3d&, ...)` in
19/// `rate_limiting.cpp`.
20fn limit_rate_vector3(
21    max_velocity: f64,
22    max_acceleration: f64,
23    max_jerk: f64,
24    commanded_velocity: &Vector3<f64>,
25    last_commanded_velocity: &Vector3<f64>,
26    last_commanded_acceleration: &Vector3<f64>,
27) -> Vector3<f64> {
28    // Differentiate to get jerk
29    let commanded_jerk = (((commanded_velocity - last_commanded_velocity) / DELTA_T)
30        - last_commanded_acceleration)
31        / DELTA_T;
32
33    // Limit jerk and integrate to get desired acceleration
34    let mut commanded_acceleration = *last_commanded_acceleration;
35    let jerk_norm = commanded_jerk.norm();
36    if jerk_norm > NORM_EPS {
37        commanded_acceleration +=
38            (commanded_jerk / jerk_norm) * cmax(cmin(jerk_norm, max_jerk), -max_jerk) * DELTA_T;
39    }
40
41    // Compute Euclidean distance to the max velocity vector that would be reached starting from
42    // last_commanded_velocity with the direction of the desired acceleration
43    let unit_commanded_acceleration = commanded_acceleration / commanded_acceleration.norm();
44    let dot_product = unit_commanded_acceleration.dot(last_commanded_velocity);
45    let distance_to_max_velocity = -dot_product
46        + (dot_product.powi(2) - last_commanded_velocity.norm_squared() + max_velocity.powi(2))
47            .sqrt();
48
49    // Compute safe acceleration limits
50    let safe_max_acceleration = cmin(
51        (max_jerk / max_acceleration) * distance_to_max_velocity,
52        max_acceleration,
53    );
54
55    // Limit acceleration and integrate to get desired velocities
56    let mut limited_commanded_velocity = *last_commanded_velocity;
57    let acceleration_norm = commanded_acceleration.norm();
58    if acceleration_norm > NORM_EPS {
59        limited_commanded_velocity +=
60            unit_commanded_acceleration * cmin(acceleration_norm, safe_max_acceleration) * DELTA_T;
61    }
62
63    limited_commanded_velocity
64}
65
66/// Limits the rate of a desired Cartesian velocity considering the limits provided.
67///
68/// Port of the `std::array<double, 6>` overload of `franka::limitRate`.
69///
70/// # Errors
71/// [`FrankaError::InvalidArgument`] if an element of `O_dP_EE_c` is infinite or NaN.
72///
73/// # Note
74/// FCI filters must be deactivated to work properly.
75#[allow(clippy::too_many_arguments)]
76pub fn limit_rate_cartesian_velocity(
77    max_translational_velocity: f64,
78    max_translational_acceleration: f64,
79    max_translational_jerk: f64,
80    max_rotational_velocity: f64,
81    max_rotational_acceleration: f64,
82    max_rotational_jerk: f64,
83    O_dP_EE_c: &[f64; 6],
84    last_O_dP_EE_c: &[f64; 6],
85    last_O_ddP_EE_c: &[f64; 6],
86) -> FrankaResult<[f64; 6]> {
87    if !O_dP_EE_c.iter().all(|v| v.is_finite()) {
88        return Err(FrankaError::InvalidArgument(
89            "O_dP_EE_c is infinite or NaN.".to_string(),
90        ));
91    }
92    let head = |v: &[f64; 6]| Vector3::new(v[0], v[1], v[2]);
93    let tail = |v: &[f64; 6]| Vector3::new(v[3], v[4], v[5]);
94
95    let translation = limit_rate_vector3(
96        max_translational_velocity,
97        max_translational_acceleration,
98        max_translational_jerk,
99        &head(O_dP_EE_c),
100        &head(last_O_dP_EE_c),
101        &head(last_O_ddP_EE_c),
102    );
103    let rotation = limit_rate_vector3(
104        max_rotational_velocity,
105        max_rotational_acceleration,
106        max_rotational_jerk,
107        &tail(O_dP_EE_c),
108        &tail(last_O_dP_EE_c),
109        &tail(last_O_ddP_EE_c),
110    );
111
112    Ok([
113        translation[0],
114        translation[1],
115        translation[2],
116        rotation[0],
117        rotation[1],
118        rotation[2],
119    ])
120}
121
122/// Limits the rate of a desired Cartesian pose considering the limits provided.
123///
124/// Port of the `std::array<double, 16>` overload of `franka::limitRate`. `O_T_EE_c` and the
125/// return value are column-major 4x4 homogeneous transformation matrices.
126///
127/// # Errors
128/// [`FrankaError::InvalidArgument`] if an element of `O_T_EE_c` is infinite or NaN, or if
129/// `O_T_EE_c` is not a homogeneous transformation matrix.
130///
131/// # Note
132/// FCI filters must be deactivated to work properly.
133#[allow(clippy::too_many_arguments)]
134pub fn limit_rate_cartesian_pose(
135    max_translational_velocity: f64,
136    max_translational_acceleration: f64,
137    max_translational_jerk: f64,
138    max_rotational_velocity: f64,
139    max_rotational_acceleration: f64,
140    max_rotational_jerk: f64,
141    O_T_EE_c: &[f64; 16],
142    last_O_T_EE_c: &[f64; 16],
143    last_O_dP_EE_c: &[f64; 6],
144    last_O_ddP_EE_c: &[f64; 6],
145) -> FrankaResult<[f64; 16]> {
146    if !O_T_EE_c.iter().all(|v| v.is_finite()) {
147        return Err(FrankaError::InvalidArgument(
148            "O_T_EE_c is infinite or NaN.".to_string(),
149        ));
150    }
151    if !is_homogeneous_transformation(O_T_EE_c) {
152        return Err(FrankaError::InvalidArgument(
153            "O_T_EE_c is invalid transformation matrix. Has to be column major!".to_string(),
154        ));
155    }
156
157    let commanded_pose = Matrix4::from_column_slice(O_T_EE_c);
158    let last_commanded_pose = Matrix4::from_column_slice(last_O_T_EE_c);
159    let commanded_translation = translation_of(&commanded_pose);
160    let last_commanded_translation = translation_of(&last_commanded_pose);
161    // Eigen's Affine3d::rotation() is the rotation factor of the polar decomposition, i.e. it
162    // orthonormalizes the linear part.
163    let commanded_rotation = orthonormalized_rotation(&linear_of(&commanded_pose));
164    let last_commanded_rotation = orthonormalized_rotation(&linear_of(&last_commanded_pose));
165
166    // Compute translational velocity
167    let translational_velocity = (commanded_translation - last_commanded_translation) / DELTA_T;
168
169    // Compute rotational velocity
170    let rot_difference = commanded_rotation * last_commanded_rotation.transpose();
171    let rotational_velocity = scaled_axis(&rot_difference) / DELTA_T;
172
173    // Limit the rate of the twist
174    let commanded_twist = [
175        translational_velocity[0],
176        translational_velocity[1],
177        translational_velocity[2],
178        rotational_velocity[0],
179        rotational_velocity[1],
180        rotational_velocity[2],
181    ];
182    let limited_twist = limit_rate_cartesian_velocity(
183        max_translational_velocity,
184        max_translational_acceleration,
185        max_translational_jerk,
186        FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE * max_rotational_velocity,
187        FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE * max_rotational_acceleration,
188        FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE * max_rotational_jerk,
189        &commanded_twist,
190        last_O_dP_EE_c,
191        last_O_ddP_EE_c,
192    )?;
193    let dx_head = Vector3::new(limited_twist[0], limited_twist[1], limited_twist[2]);
194    let dx_tail = Vector3::new(limited_twist[3], limited_twist[4], limited_twist[5]);
195
196    // Integrate limited twist
197    let limited_translation = last_commanded_translation + dx_head * DELTA_T;
198    let mut limited_rotation = last_commanded_rotation;
199    let angular_speed = dx_tail.norm();
200    if angular_speed > NORM_EPS {
201        let w_norm = dx_tail / angular_speed;
202        let theta = DELTA_T * angular_speed;
203        let omega_skew = Matrix3::new(
204            0.0, -w_norm[2], w_norm[1], //
205            w_norm[2], 0.0, -w_norm[0], //
206            -w_norm[1], w_norm[0], 0.0,
207        );
208        let rotation = Matrix3::identity()
209            + theta.sin() * omega_skew
210            + (1.0 - theta.cos()) * (omega_skew * omega_skew);
211        limited_rotation = rotation * last_commanded_rotation;
212    }
213
214    Ok(pose_to_array(&limited_rotation, &limited_translation))
215}
216
217/// `axis * angle` of a rotation matrix.
218///
219/// Port of `Eigen::AngleAxisd(rotation).axis() * Eigen::AngleAxisd(rotation).angle()`, including
220/// Eigen's quaternion-based extraction (which yields the zero vector for the identity rotation).
221pub(crate) fn scaled_axis(rotation: &Matrix3<f64>) -> Vector3<f64> {
222    let q = UnitQuaternion::from_rotation_matrix(&Rotation3::from_matrix_unchecked(*rotation));
223    let vec = Vector3::new(q.i, q.j, q.k);
224    let mut n = vec.norm();
225    if n == 0.0 {
226        return Vector3::zeros();
227    }
228    let angle = 2.0 * n.atan2(q.w.abs());
229    if q.w < 0.0 {
230        n = -n;
231    }
232    (vec / n) * angle
233}
franka/rate_limiting/cartesian.rs

franka/rate_limiting/
cartesian.rs
