franka/robot/target_control/
runner.rs

1//! The realtime side of [target control](super), generic over the interface: anchoring, the
2//! slot, the generator under the three rules, the deviation guard, and the stop path --
3//! land, hold the echo of the last command for the settle window, finish on one more of it.
4
5use std::sync::atomic::Ordering;
6use std::sync::mpsc::SyncSender;
7use std::sync::Arc;
8
9use super::{Settle, Shared, DEVIATION_MESSAGE, STOP_TIMEOUT_CYCLES};
10use crate::error::{ControlException, FrankaError, FrankaResult};
11use crate::otg::{MultiOtg, OtgLimits};
12use crate::rate_limiting::DELTA_T;
13use crate::robot_state::RobotState;
14
15/// A generator within [`Settle::tolerance`] of its target moving slower than this, per axis,
16/// counts as landed (m/s, rad/s). The hold then freezes a velocity step of at most this in
17/// one cycle, a jerk of 100 per second cubed, which the joint side of a Cartesian command
18/// amplifies threefold (1 mm/s froze as 3840 rad/s^3 on joint 2 in the simulator, over its
19/// 3750). Not smaller: re-anchoring on a float32 echo (FCI v10) keeps a landed generator in
20/// micro-profiles of a few 1e-8 that peak at about 2e-5 per second.
21pub const REST_VELOCITY: f64 = 1e-4;
22/// ... and accelerating less than this (m/s^2, rad/s^2); the micro-profiles above peak at
23/// about 0.01.
24pub const REST_ACCELERATION: f64 = 0.05;
25/// An *arm* whose every joint moves slower than this, rad/s, is at rest: what a stop in torque
26/// mode waits for before `motion_finished`, the hold having settled only the generator.
27pub const REST_JOINT_VELOCITY: f64 = 0.01;
28
29/// Maps the slot's target and the robot's echo (`S` values each) to where the generator is
30/// re-anchored and where it is to go (`N` values each) for one cycle.
31pub(super) type Chart<const N: usize, const S: usize> =
32    fn(target: &[f64; S], commanded: &[f64; S]) -> ([f64; N], [f64; N]);
33
34/// What one cycle produced.
35pub(super) struct Step<const N: usize, const S: usize> {
36    pub position: [f64; N],
37    pub velocity: [f64; N],
38    pub acceleration: [f64; N],
39    /// The slot's target the generator was planning towards.
40    pub target: [f64; S],
41    /// The command is being held bit-identical; the backstop must not touch it.
42    pub hold: bool,
43    pub finished: bool,
44}
45
46/// The stop's final phase: the same position, sent `sent` times so far.
47struct Hold<const N: usize> {
48    position: [f64; N],
49    sent: u32,
50}
51
52/// See the [module documentation](self).
53pub(super) struct Runner<const N: usize, const S: usize> {
54    shared: Arc<Shared<S>>,
55    started: Option<SyncSender<()>>,
56    otg: MultiOtg<N>,
57    settle: Settle,
58    chart: Chart<N, S>,
59    target: [f64; S],
60    anchored: bool,
61    deviated: bool,
62    stop_cycles: u32,
63    hold: Option<Hold<N>>,
64}
65
66/// The chart of an interface whose slot and generator share their coordinates.
67pub(super) fn identity<const N: usize>(
68    target: &[f64; N],
69    commanded: &[f64; N],
70) -> ([f64; N], [f64; N]) {
71    (*commanded, *target)
72}
73
74impl<const N: usize, const S: usize> Runner<N, S> {
75    pub(super) fn new(
76        shared: Arc<Shared<S>>,
77        started: SyncSender<()>,
78        limits: [OtgLimits; N],
79        settle: Settle,
80        chart: Chart<N, S>,
81    ) -> FrankaResult<Self> {
82        Ok(Runner {
83            shared,
84            started: Some(started),
85            // Synchronised axes: a diagonal target moves along a straight line, and a pose
86            // target's translation and rotation arrive together.
87            otg: MultiOtg::with_limits([0.0; N], limits, true)?,
88            settle,
89            chart,
90            target: [0.0; S],
91            anchored: false,
92            deviated: false,
93            stop_cycles: 0,
94            hold: None,
95        })
96    }
97
98    /// One cycle: `commanded` is the robot's echo of the last command and `strayed` whether
99    /// the deviation guard's threshold is crossed (`false` until anchored).
100    pub(super) fn cycle(
101        &mut self,
102        state: &RobotState,
103        commanded: [f64; S],
104        strayed: bool,
105    ) -> Step<N, S> {
106        if let Ok(mut latest) = self.shared.state.try_lock() {
107            *latest = *state;
108        }
109        if let Some(hold) = &mut self.hold {
110            // Neither re-anchored nor stepped: the very same command again, and
111            // `motion_finished` on the one after the settle window.
112            hold.sent += 1;
113            return Step {
114                position: hold.position,
115                velocity: [0.0; N],
116                acceleration: [0.0; N],
117                target: self.target,
118                hold: true,
119                finished: hold.sent > self.settle.cycles,
120            };
121        }
122        // Read before the slot: a target published before `stop()` is then never missed.
123        let stopping = self.deviated || self.shared.stop.load(Ordering::SeqCst);
124        if !self.anchored {
125            self.anchored = true;
126            self.target = commanded;
127            self.otg.reset((self.chart)(&commanded, &commanded).0);
128            self.shared.slot.publish(commanded);
129            if let Some(started) = self.started.take() {
130                let _ = started.try_send(());
131            }
132        } else if !self.deviated && strayed {
133            self.deviated = true;
134            self.target = commanded;
135        } else if !self.deviated {
136            // A torn read keeps the previous target for this one cycle; a stop, which no
137            // publish can follow, waits for the final one.
138            while !self.shared.slot.load(&mut self.target) && stopping {}
139        }
140        // The echo and the target are finite, so neither setter fails.
141        let (anchor, goal) = (self.chart)(&self.target, &commanded);
142        let _ = self.otg.set_position(anchor);
143        let _ = self.otg.set_target(goal);
144        let position = self.otg.step(DELTA_T);
145        let axes = self.otg.axes();
146        let velocity = std::array::from_fn(|i| axes[i].velocity());
147        let acceleration = std::array::from_fn(|i| axes[i].acceleration());
148
149        if stopping || self.deviated {
150            self.stop_cycles = self.stop_cycles.saturating_add(1);
151            let landed = axes.iter().all(|a| {
152                (a.position() - a.target()).abs() < self.settle.tolerance
153                    && a.velocity().abs() < REST_VELOCITY
154                    && a.acceleration().abs() < REST_ACCELERATION
155            });
156            // Landed, or out of patience: hold the robot's echo of the last command, which
157            // is continuous with what the robot already has by construction (the generator's
158            // own position may differ from it by whatever the backstop took off the last
159            // command), and this is the first of the identical ones.
160            if landed || self.stop_cycles >= STOP_TIMEOUT_CYCLES {
161                let held = (self.chart)(&commanded, &commanded).0;
162                self.hold = Some(Hold {
163                    position: held,
164                    sent: 1,
165                });
166                return Step {
167                    position: held,
168                    velocity: [0.0; N],
169                    acceleration: [0.0; N],
170                    target: self.target,
171                    hold: true,
172                    finished: false,
173                };
174            }
175        }
176        Step {
177            position,
178            velocity,
179            acceleration,
180            target: self.target,
181            hold: false,
182            finished: false,
183        }
184    }
185
186    /// The loop's result: a regular end after the deviation guard fired is the error it is.
187    pub(super) fn finish(&self, result: FrankaResult<()>) -> FrankaResult<()> {
188        match result {
189            Ok(()) if self.deviated => Err(FrankaError::Control(ControlException::new(
190                DEVIATION_MESSAGE,
191            ))),
192            other => other,
193        }
194    }
195}
franka/robot/target_control/runner.rs

franka/robot/target_control/
runner.rs
