franka/robot/target_control/
joint.rs

1//! The joint interface of [target control](super): seven joint position targets.
2//! [`Backend::Impedance`] sends the impedance law's torques towards the generator's output;
3//! [`Backend::RobotController`] streams it through [`Robot::control_joint_positions`].
4
5use std::sync::mpsc::SyncSender;
6use std::sync::Arc;
7
8use super::position::{JointLimits, VelocityLimit};
9use super::runner::{identity, Step};
10use super::torque::{JointTracker, TorqueLoop};
11use super::{
12    check_joint_limits, check_posture, joint_limit_inset, joint_position_limits, spawn, Backend,
13    Handle, ImpedanceOptions, JointTargetControlOptions, Runner, Shared, DEFAULT_LIMIT_FRACTION,
14};
15use crate::control_types::JointPositions;
16use crate::error::FrankaResult;
17use crate::lowpass_filter::MAX_CUTOFF_FREQUENCY;
18use crate::model::Model;
19use crate::otg::OtgLimits;
20use crate::rate_limiting::limit_rate_joint_positions;
21use crate::robot::Robot;
22use crate::robot_state::RobotState;
23use crate::wire::robot::codec::FciVersion;
24
25/// What one cycle sent, for the observer.
26#[derive(Debug, Clone, Copy, PartialEq)]
27pub struct JointSent {
28    /// The joint positions that went to the robot (after the backstop), or with
29    /// [`Backend::Impedance`] the generator's output the law tracks, rad.
30    pub q: [f64; 7],
31    /// The target the generator was planning towards, rad.
32    pub target: [f64; 7],
33    /// The generator's velocity at the end of the cycle, rad/s.
34    pub velocity: [f64; 7],
35    /// The generator's acceleration at the end of the cycle, rad/s^2.
36    pub acceleration: [f64; 7],
37    /// The most the backstop moved any joint of the generator's position, rad; zero without
38    /// `limit_rate` and with [`Backend::Impedance`]. Microradians are the noise of the
39    /// limiter's velocity and acceleration reference, the robot's float32 echo (FCI v10);
40    /// milliradians mean the generator is outrunning the limits.
41    pub backstop_alteration: f64,
42    /// The joint target of the impedance law, rad; zeros with [`Backend::RobotController`].
43    pub q_goal: [f64; 7],
44    /// The joint goal's velocity, rad/s: the finite difference of `q_goal` the law feeds
45    /// forward, under the joint velocity cap; zeros while holding and with
46    /// [`Backend::RobotController`].
47    pub dq_goal: [f64; 7],
48    /// The scale this cycle's goal step was cut by to stay in the joint limits' box: under
49    /// [`ImpedanceOptions::joint_velocity_fraction`](super::ImpedanceOptions::joint_velocity_fraction)
50    /// of the joint velocity limits, and braking to stop at
51    /// [`ImpedanceOptions::joint_position_margin`](super::ImpedanceOptions::joint_position_margin).
52    /// Exactly 1 when it was not, and with [`Backend::RobotController`].
53    pub cap_scale: f64,
54    /// Per joint, the bound that cut the step: 0 none, -1 / +1 the lower / upper position bound,
55    /// -2 / +2 the velocity bound; zeros with [`Backend::RobotController`].
56    pub pinned: [i8; 7],
57    /// The torques sent, Nm, clamped to the torque limits; zeros with
58    /// [`Backend::RobotController`].
59    pub tau: [f64; 7],
60    /// The velocity envelope's share of `tau`, Nm, before the clamp: the barrier opposing every
61    /// joint measured faster than
62    /// [`ImpedanceOptions::velocity_barrier_fraction`](super::ImpedanceOptions::velocity_barrier_fraction)
63    /// of its limit, less the law's torque along a joint's motion faded out above the fade's
64    /// start ([`FADE_BAND`](super::FADE_BAND)); zeros below it and with
65    /// [`Backend::RobotController`].
66    pub tau_envelope: [f64; 7],
67    /// The position envelope's share of `tau`, Nm, before the clamp; see
68    /// [`CartesianSent::tau_position`](super::CartesianSent::tau_position). Zeros with
69    /// [`Backend::RobotController`].
70    pub tau_position: [f64; 7],
71    /// The most, rad, the leash pulled any joint of the generator's anchor back from the
72    /// previous goal toward the measured position: zero while the arm follows, positive while
73    /// it is held back ([`Leash`](super::Leash)); 0 with [`Backend::RobotController`].
74    pub leash_alteration: f64,
75}
76
77/// The observer's type: called every cycle on the realtime thread, so it must not allocate
78/// or block.
79pub type JointObserver = Box<dyn FnMut(&RobotState, &JointSent) + Send>;
80
81/// The handle of a running joint target control; see the [module documentation](super).
82///
83/// It has no live tuning, and no `tune`: [`LiveTuning`](super::LiveTuning) describes the
84/// Cartesian interface's law and plan -- a Cartesian stiffness this interface has none of, and a
85/// budget as a norm, where this one plans per joint. A joint session therefore runs the options
86/// it was started with for its whole life.
87pub struct JointTargetControl {
88    inner: Handle<7>,
89    limits: ([f64; 7], [f64; 7]),
90    /// How far inside `limits` a target must lie.
91    inset: f64,
92}
93
94impl JointTargetControl {
95    /// Sets the target joint positions, rad. Callable from any thread at any rate; only the
96    /// latest target counts.
97    ///
98    /// # Errors
99    /// [`crate::error::FrankaError::InvalidArgument`] if a value is not finite or outside the
100    /// arm's joint position limits inset by [`JOINT_LIMIT_INSET`](super::JOINT_LIMIT_INSET) (with
101    /// [`Backend::Impedance`], by its
102    /// [`joint_position_margin`](ImpedanceOptions::joint_position_margin) where that is larger),
103    /// [`crate::error::FrankaError::InvalidOperation`] with [`super::ENDED_MESSAGE`] once the
104    /// loop has ended for any reason.
105    pub fn set_joints(&self, q: [f64; 7]) -> FrankaResult<()> {
106        if q.iter().all(|v| v.is_finite()) {
107            check_joint_limits(&q, &self.limits, self.inset, "target")?;
108        }
109        self.inner.set_target(q)
110    }
111
112    /// The latest target set (the start configuration until the first `set_joints`).
113    pub fn target(&self) -> [f64; 7] {
114        self.inner.target()
115    }
116
117    /// The latest robot state the loop received, copied out.
118    pub fn state(&self) -> RobotState {
119        self.inner.state()
120    }
121
122    /// Whether the loop is still running; `false` after it ended for any reason.
123    pub fn is_running(&self) -> bool {
124        self.inner.is_running()
125    }
126
127    /// Settles at the current target, finishes the motion, joins the thread and returns the
128    /// loop's result: `Ok` for a regular end, [`crate::error::FrankaError::Control`] if the
129    /// robot aborted the motion or the deviation guard fired.
130    pub fn stop(self) -> FrankaResult<()> {
131        self.inner.stop()
132    }
133}
134
135pub(super) fn max_abs_difference(a: &[f64; 7], b: &[f64; 7]) -> f64 {
136    a.iter()
137        .zip(b)
138        .map(|(x, y)| (x - y).abs())
139        .fold(0.0, f64::max)
140}
141
142/// The observer's record of a step with `q` as sent; the backend's own fields stay zero.
143pub(super) fn sent(step: &Step<7, 7>, q: [f64; 7]) -> JointSent {
144    JointSent {
145        q,
146        target: step.target,
147        velocity: step.velocity,
148        acceleration: step.acceleration,
149        backstop_alteration: 0.0,
150        q_goal: [0.0; 7],
151        dq_goal: [0.0; 7],
152        cap_scale: 1.0,
153        pinned: [0; 7],
154        tau: [0.0; 7],
155        tau_envelope: [0.0; 7],
156        tau_position: [0.0; 7],
157        leash_alteration: 0.0,
158    }
159}
160
161const THREAD: &str = "franka-joint-target";
162
163pub(super) fn start(
164    robot: &Arc<Robot>,
165    options: JointTargetControlOptions,
166) -> FrankaResult<JointTargetControl> {
167    options.validate()?;
168    let joint_limits = joint_position_limits(robot.fci_version());
169    check_posture(&options.backend, &joint_limits)?;
170    let inset = joint_limit_inset(&options.backend);
171    let limits = options.limits.unwrap_or_else(|| {
172        JointTargetControlOptions::scaled_limits(robot.fci_version(), DEFAULT_LIMIT_FRACTION)
173    });
174    let shared = Arc::new(Shared::<7>::default());
175    let loop_shared = Arc::clone(&shared);
176    let scheduling = options.scheduling();
177    let inner = match options.backend {
178        Backend::RobotController => spawn(
179            THREAD,
180            robot,
181            shared,
182            scheduling,
183            move |robot: &Robot, started| {
184                position_loop(robot, started, options, limits, loop_shared)
185            },
186        )?,
187        Backend::Impedance(impedance) => {
188            let model = Arc::new(robot.load_model()?);
189            spawn(
190                THREAD,
191                robot,
192                shared,
193                scheduling,
194                move |robot: &Robot, started| {
195                    let limit_rate = options.limit_rate;
196                    let version = robot.fci_version();
197                    torque_loop(
198                        options,
199                        limits,
200                        impedance,
201                        model,
202                        version,
203                        loop_shared,
204                        started,
205                    )?
206                    .run(robot, limit_rate)
207                },
208            )?
209        }
210    };
211    Ok(JointTargetControl {
212        inner,
213        limits: joint_limits,
214        inset,
215    })
216}
217
218/// The [`Backend::Impedance`] loop of this interface: a [`JointTracker`] on the seven-joint
219/// runner under `limits`, boxed in the version's joint limits.
220pub(super) fn torque_loop(
221    options: JointTargetControlOptions,
222    limits: [OtgLimits; 7],
223    impedance: ImpedanceOptions,
224    model: Arc<Model>,
225    version: FciVersion,
226    shared: Arc<Shared<7>>,
227    started: SyncSender<()>,
228) -> FrankaResult<TorqueLoop<7, 7, JointTracker>> {
229    let runner = Runner::new(shared, started, limits, options.settle, identity)?;
230    let velocity = VelocityLimit::of(version);
231    let limits = JointLimits {
232        position: joint_position_limits(version),
233        margin: impedance.joint_position_margin,
234        fraction: impedance.joint_velocity_fraction,
235        velocity,
236    };
237    let tracker = JointTracker::new(&options, limits);
238    // No live tuning: `LiveTuning`'s budget is a Cartesian norm, which this interface plans
239    // nothing with, and the fields of it this tracker would keep a copy of -- the IK's, the
240    // feedforward's filter -- it has none of. Its slot stays unseeded and its handle offers no
241    // setter, so the loop reads nothing per cycle.
242    Ok(TorqueLoop::new(
243        runner,
244        model,
245        impedance,
246        tracker,
247        options.observer,
248        None,
249    ))
250}
251
252/// [`Backend::RobotController`]: the position stream, re-anchored on the robot's echo every
253/// cycle with the rate limiter as the backstop; a stop holds the echo bit for bit.
254fn position_loop(
255    robot: &Robot,
256    started: SyncSender<()>,
257    options: JointTargetControlOptions,
258    limits: [OtgLimits; 7],
259    shared: Arc<Shared<7>>,
260) -> FrankaResult<()> {
261    let JointTargetControlOptions {
262        controller_mode,
263        max_deviation,
264        settle,
265        limit_rate,
266        mut observer,
267        ..
268    } = options;
269    let mut runner = Runner::new(shared, started, limits, settle, identity)?;
270    let velocity = limits.map(|l| l.max_velocity);
271    let acceleration = limits.map(|l| l.max_acceleration);
272    let jerk = limits.map(|l| l.max_jerk);
273    let mut start: Option<[f64; 7]> = None;
274    let result = robot.control_joint_positions(
275        |state: &RobotState, _period| {
276            let start = *start.get_or_insert(state.q_d);
277            let strayed = max_abs_difference(&state.q, &start) > max_deviation;
278            let step = runner.cycle(state, state.q_d, strayed, false);
279
280            let mut q = step.position;
281            let mut backstop_alteration = 0.0;
282            if limit_rate && !step.hold {
283                // The budget, tightened to the robot's own velocity envelope at `q`
284                // (position-dependent on an FR3), against the robot's echo; it only fails
285                // on non-finite input, and holding the echo is then the safe command.
286                // Skipped while the stop holds: that command must go out bit-identical.
287                let upper = robot.upper_joint_velocity_limits(&state.q);
288                let lower = robot.lower_joint_velocity_limits(&state.q);
289                let upper: [f64; 7] = std::array::from_fn(|i| upper[i].min(velocity[i]));
290                let lower: [f64; 7] = std::array::from_fn(|i| lower[i].max(-velocity[i]));
291                #[rustfmt::skip]
292                let limited = limit_rate_joint_positions(
293                    &upper, &lower, &acceleration, &jerk,
294                    &q, &state.q_d, &state.dq_d, &state.ddq_d,
295                ).unwrap_or(state.q_d);
296                backstop_alteration = max_abs_difference(&limited, &q);
297                q = limited;
298            }
299            if let Some(observe) = observer.as_mut() {
300                observe(
301                    state,
302                    &JointSent {
303                        backstop_alteration,
304                        ..sent(&step, q)
305                    },
306                );
307            }
308            let mut output = JointPositions::new(q);
309            output.motion_finished = step.finished;
310            output
311        },
312        controller_mode,
313        limit_rate,
314        MAX_CUTOFF_FREQUENCY,
315    );
316    runner.finish(result)
317}
franka/robot/target_control/joint.rs

franka/robot/target_control/
joint.rs
