franka/rate_limiting/
fer.rs

1//! Rate limits of the Franka Emika Robot (FER, FCI v5), ported from libfranka 0.9.2
2//! `include/franka/rate_limiting.h`.
3//!
4//! The constants in [the parent module](super) are the FR3 values of libfranka 0.21.2; the FER
5//! uses different ones, most notably `kTolNumberPacketsLost = 3.0` (the FR3 expects no packet
6//! loss and uses `0.0`), which shows up in every velocity limit. The limiting *functions*
7//! themselves are unchanged — they take their bounds as arguments — so a v5 control loop calls
8//! the same [`super::limit_rate_torques`], [`super::limit_rate_joint_velocities`], … with the constants from
9//! this module.
10//!
11//! Unlike the FR3, the FER's joint velocity limits are flat: there is no position-dependent
12//! envelope and no URDF to read one from, so [`MAX_JOINT_VELOCITY`] is used directly in
13//! both directions.
14
15/// Sample time constant. Port of `franka::kDeltaT` (`rate_limiting.h:18`). Same as on FR3.
16pub const DELTA_T: f64 = 1e-3;
17/// Epsilon value for checking limits. Port of `franka::kLimitEps` (`rate_limiting.h:22`).
18/// Same as on FR3.
19pub const LIMIT_EPS: f64 = 1e-3;
20/// Epsilon value for limiting Cartesian accelerations/jerks or not. Port of
21/// `franka::kNormEps` (`rate_limiting.h:26`). Same as on FR3.
22pub const NORM_EPS: f64 = f64::EPSILON;
23/// Number of packets lost considered for the definition of velocity limits. When a packet
24/// is lost, FCI assumes a constant acceleration model. Port of
25/// `franka::kTolNumberPacketsLost` (`rate_limiting.h:31`) — `3.0` on the FER, `0.0` on
26/// the FR3.
27pub const TOL_NUMBER_PACKETS_LOST: f64 = 3.0;
28/// Factor for the definition of rotational limits using the Cartesian pose interface. Port
29/// of `franka::kFactorCartesianRotationPoseInterface` (`rate_limiting.h:35`). Same as on
30/// FR3.
31pub const FACTOR_CARTESIAN_ROTATION_POSE_INTERFACE: f64 = 0.99;
32
33/// Maximum torque rate. Port of `franka::kMaxTorqueRate` (`rate_limiting.h:39-41`).
34pub const MAX_TORQUE_RATE: [f64; 7] = [1000.0 - LIMIT_EPS; 7];
35/// Maximum joint jerk. Port of `franka::kMaxJointJerk` (`rate_limiting.h:45-47`).
36pub const MAX_JOINT_JERK: [f64; 7] = [
37    7500.0 - LIMIT_EPS,
38    3750.0 - LIMIT_EPS,
39    5000.0 - LIMIT_EPS,
40    6250.0 - LIMIT_EPS,
41    7500.0 - LIMIT_EPS,
42    10000.0 - LIMIT_EPS,
43    10000.0 - LIMIT_EPS,
44];
45/// Maximum joint acceleration. Port of `franka::kMaxJointAcceleration`
46/// (`rate_limiting.h:51-53`).
47pub const MAX_JOINT_ACCELERATION: [f64; 7] = [
48    15.0000 - LIMIT_EPS,
49    7.500 - LIMIT_EPS,
50    10.0000 - LIMIT_EPS,
51    12.5000 - LIMIT_EPS,
52    15.0000 - LIMIT_EPS,
53    20.0000 - LIMIT_EPS,
54    20.0000 - LIMIT_EPS,
55];
56/// Maximum joint velocity. Port of `franka::kMaxJointVelocity` (`rate_limiting.h:57-64`).
57///
58/// Flat, unlike the FR3's position-dependent envelope: joints 1-4 start from 2.175 rad/s,
59/// joints 5-7 from 2.610 rad/s, each reduced by `kLimitEps` and by the velocity the joint
60/// could pick up over three lost packets.
61pub const MAX_JOINT_VELOCITY: [f64; 7] = [
62    2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[0],
63    2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[1],
64    2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[2],
65    2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[3],
66    2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[4],
67    2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[5],
68    2.6100 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_JOINT_ACCELERATION[6],
69];
70/// Minimum joint velocity, i.e. `-MAX_JOINT_VELOCITY`.
71///
72/// libfranka 0.9.2 has no such array — its `limitRate` overloads take a single symmetric
73/// `max_velocity` — but the crate's [`super::limit_rate_joint_velocities`] takes an upper
74/// and a lower bound, so the negated array is what a v5 control loop passes as the lower
75/// one.
76pub const MIN_JOINT_VELOCITY: [f64; 7] = [
77    -MAX_JOINT_VELOCITY[0],
78    -MAX_JOINT_VELOCITY[1],
79    -MAX_JOINT_VELOCITY[2],
80    -MAX_JOINT_VELOCITY[3],
81    -MAX_JOINT_VELOCITY[4],
82    -MAX_JOINT_VELOCITY[5],
83    -MAX_JOINT_VELOCITY[6],
84];
85/// Maximum translational jerk. Port of `franka::kMaxTranslationalJerk`
86/// (`rate_limiting.h:68`).
87pub const MAX_TRANSLATIONAL_JERK: f64 = 6500.0 - LIMIT_EPS;
88/// Maximum translational acceleration. Port of `franka::kMaxTranslationalAcceleration`
89/// (`rate_limiting.h:72`).
90pub const MAX_TRANSLATIONAL_ACCELERATION: f64 = 13.0000 - LIMIT_EPS;
91/// Maximum translational velocity. Port of `franka::kMaxTranslationalVelocity`
92/// (`rate_limiting.h:76-77`).
93pub const MAX_TRANSLATIONAL_VELOCITY: f64 =
94    2.0000 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_TRANSLATIONAL_ACCELERATION;
95/// Maximum rotational jerk. Port of `franka::kMaxRotationalJerk` (`rate_limiting.h:81`).
96pub const MAX_ROTATIONAL_JERK: f64 = 12500.0 - LIMIT_EPS;
97/// Maximum rotational acceleration. Port of `franka::kMaxRotationalAcceleration`
98/// (`rate_limiting.h:85`).
99pub const MAX_ROTATIONAL_ACCELERATION: f64 = 25.0000 - LIMIT_EPS;
100/// Maximum rotational velocity. Port of `franka::kMaxRotationalVelocity`
101/// (`rate_limiting.h:89-90`).
102pub const MAX_ROTATIONAL_VELOCITY: f64 =
103    2.5000 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ROTATIONAL_ACCELERATION;
104/// Maximum elbow jerk. Port of `franka::kMaxElbowJerk` (`rate_limiting.h:94`).
105pub const MAX_ELBOW_JERK: f64 = 5000.0 - LIMIT_EPS;
106/// Maximum elbow acceleration. Port of `franka::kMaxElbowAcceleration`
107/// (`rate_limiting.h:98`).
108pub const MAX_ELBOW_ACCELERATION: f64 = 10.0000 - LIMIT_EPS;
109/// Maximum elbow velocity. Port of `franka::kMaxElbowVelocity` (`rate_limiting.h:102-103`).
110pub const MAX_ELBOW_VELOCITY: f64 =
111    2.1750 - LIMIT_EPS - TOL_NUMBER_PACKETS_LOST * DELTA_T * MAX_ELBOW_ACCELERATION;
112/// Minimum elbow velocity, i.e. `-MAX_ELBOW_VELOCITY`.
113///
114/// libfranka 0.9.2 has no `kMinElbowVelocity`; its `limitRate(max_velocity, ...)` overload
115/// is symmetric (`src/control_loop.cpp:254`). 0.21.2 spells the negation out as a constant
116/// and the crate follows it, so the v5 module defines the same derived value.
117pub const MIN_ELBOW_VELOCITY: f64 = -MAX_ELBOW_VELOCITY;
118/// Joint position limits (lower, upper), rad: the `<limit>` of `joint1..7` in the FER URDF
119/// the crate compiles in ([`crate::model::FER_URDF`], `tests/data/fer.urdf`).
120pub const JOINT_POSITION_LIMITS: ([f64; 7], [f64; 7]) = (
121    [
122        -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973,
123    ],
124    [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973],
125);
franka/rate_limiting/fer.rs

franka/rate_limiting/
fer.rs
