franka/robot/target_control/
velocity.rs

1//! The torque backend's joint velocity envelope: the cap on the joint goal's step, which keeps
2//! what the loop commands under a fraction of the arm's velocity limits; the fade, which takes
3//! the law's push off a joint measured faster than the cap (or [`FADE_BAND`] under the barrier's
4//! onset), so that catching up on its lag does not drive it past that onset; and the barrier on
5//! the measured velocity, which damps what the two cannot: spring-back and overshoot.
6
7use super::impedance::TORQUE_LIMITS;
8
9/// Scales `to - from` as a whole by the largest `s ≤ 1` that keeps every joint's step within
10/// `[lower, upper]` (`lower ≤ 0 ≤ upper`), which keeps the step's direction in joint space;
11/// returns `s`.
12pub(super) fn scale_step(
13    from: &[f64; 7],
14    to: &mut [f64; 7],
15    lower: &[f64; 7],
16    upper: &[f64; 7],
17) -> f64 {
18    let mut scale = 1.0f64;
19    for i in 0..7 {
20        let step = to[i] - from[i];
21        if step > upper[i] {
22            scale = scale.min(upper[i] / step);
23        } else if step < lower[i] {
24            scale = scale.min(lower[i] / step);
25        }
26    }
27    if scale < 1.0 {
28        for (q, from) in to.iter_mut().zip(from) {
29            *q = from + scale * (*q - from);
30        }
31    }
32    scale
33}
34
35/// [`scale_step`] within `±max_step`.
36#[cfg(test)]
37pub(super) fn cap_step(from: &[f64; 7], to: &mut [f64; 7], max_step: &[f64; 7]) -> f64 {
38    scale_step(from, to, &max_step.map(|m| -m), max_step)
39}
40
41/// Nm per rad/s of the velocity barrier: the damping a joint meets beyond
42/// [`ImpedanceOptions::velocity_barrier_fraction`](super::ImpedanceOptions::velocity_barrier_fraction)
43/// of its limit. Stable at 1 kHz behind the 100 Hz low-pass while the law's own joint damping
44/// plus this stays under the joint's effective inertia per millisecond; see *The impedance
45/// backend* in the book.
46pub const VELOCITY_BARRIER_GAIN: f64 = 20.0;
47
48/// The narrowest band, as a fraction of a joint's velocity limit, over which the law's push
49/// along the joint's motion fades out under the barrier's onset: the fade starts at the cap or
50/// this far under the onset, whichever is lower, and never below rest.
51///
52/// Away from a position limit the band is this wide and the fade adds up to
53/// `|law| / (FADE_BAND × limit)` Nm per rad/s to the loop's gain, about 30 on an FER's wrist at
54/// its 11.5 Nm clamp, which the loop settles on 0.03 kg m² and more; a fade from a cap of 0.8
55/// rings there. Inside the position margin the cap is 0 -- the braking envelope is 0 at and past
56/// the margin -- so the start is 0 and the band is `[0, onset]`. Where the arm's limit toward
57/// that side is flat, as the FER's is, that is the narrower band and the figure above is not the
58/// bound; what bounds the gain is the position fade
59/// ([`POSITION_FADE_BAND`](super::POSITION_FADE_BAND)) tapering the same push to nothing over
60/// the same rad. Both vanish linearly, so the product is finite, and it peaks at the margin at
61/// `|law| / (velocity_barrier_fraction × E(POSITION_FADE_BAND))`: on the FER 1.16 to 1.61 times
62/// the figure above (`tests/torque_position.rs`). The FR3's limit is position-dependent and has
63/// itself come down by the margin, so its band is wider there and the figure above does bound
64/// it.
65pub const FADE_BAND: f64 = 0.15;
66
67/// Where a joint's fade starts, rad/s: at `cap`, or [`FADE_BAND`] of `limit` under `onset` if
68/// that is lower, and never below rest, where a barrier under the band would otherwise fade the
69/// law at every speed.
70pub(super) fn fade_start(limit: f64, cap: f64, onset: f64) -> f64 {
71    cap.min(onset - FADE_BAND * limit).max(0.0)
72}
73
74/// `law` with every joint's torque along its measured motion faded out linearly as the joint
75/// speeds from `start` to `onset` (above it): in full at or below the start, none at or above
76/// the onset. A lagging joint's catch-up would otherwise cancel the barrier, and the ramp
77/// starts early enough for the torque that reaches the arm, behind the low-pass, to have mostly
78/// faded by the onset. Torque against the motion is never scaled; the law's Coriolis term is
79/// part of `law` and fades with it.
80pub(super) fn fade_push(
81    law: &[f64; 7],
82    dq: &[f64; 7],
83    start: &[f64; 7],
84    onset: &[f64; 7],
85) -> [f64; 7] {
86    std::array::from_fn(|i| {
87        let speed = dq[i].abs();
88        if law[i] * dq[i] > 0.0 && speed > start[i] {
89            law[i] * ((onset[i] - speed) / (onset[i] - start[i])).clamp(0.0, 1.0)
90        } else {
91            law[i]
92        }
93    })
94}
95
96/// The barrier's gain per joint, Nm per rad/s: [`VELOCITY_BARRIER_GAIN`], lowered in proportion
97/// where `torque_limits` clamps a joint below the default clamp (86 Nm on joints 1-4, 11.5 on
98/// 5-7), so a lighter clamp does not leave a stiffer barrier to ring on the joint's inertia.
99pub(super) fn barrier_gains(torque_limits: &[f64; 7]) -> [f64; 7] {
100    std::array::from_fn(|i| VELOCITY_BARRIER_GAIN * (torque_limits[i] / TORQUE_LIMITS[i]).min(1.0))
101}
102
103/// The barrier's torque, Nm: `gains` per rad/s by which a joint's measured velocity exceeds
104/// `onset`, opposing it and at most that joint's torque limit; zero below.
105pub(super) fn velocity_barrier(
106    dq: &[f64; 7],
107    onset: &[f64; 7],
108    gains: &[f64; 7],
109    torque_limits: &[f64; 7],
110) -> [f64; 7] {
111    std::array::from_fn(|i| {
112        let excess = dq[i].abs() - onset[i];
113        if excess > 0.0 {
114            (gains[i] * excess).min(torque_limits[i]).copysign(-dq[i])
115        } else {
116            0.0
117        }
118    })
119}
franka/robot/target_control/velocity.rs

franka/robot/target_control/
velocity.rs
