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::runner::{identity, Step};
9use super::torque::{JointTracker, TorqueLoop};
10use super::{
11    check_joint_limits, check_posture, joint_position_limits, spawn, Backend, Handle,
12    ImpedanceOptions, JointTargetControlOptions, Runner, Shared, DEFAULT_LIMIT_FRACTION,
13};
14use crate::control_types::JointPositions;
15use crate::error::FrankaResult;
16use crate::lowpass_filter::MAX_CUTOFF_FREQUENCY;
17use crate::model::Model;
18use crate::otg::OtgLimits;
19use crate::rate_limiting::limit_rate_joint_positions;
20use crate::robot::Robot;
21use crate::robot_state::RobotState;
22
23/// What one cycle sent, for the observer.
24#[derive(Debug, Clone, Copy, PartialEq)]
25pub struct JointSent {
26    /// The joint positions that went to the robot (after the backstop), or with
27    /// [`Backend::Impedance`] the generator's output the law tracks, rad.
28    pub q: [f64; 7],
29    /// The target the generator was planning towards, rad.
30    pub target: [f64; 7],
31    /// The generator's velocity at the end of the cycle, rad/s.
32    pub velocity: [f64; 7],
33    /// The generator's acceleration at the end of the cycle, rad/s^2.
34    pub acceleration: [f64; 7],
35    /// The most the backstop moved any joint of the generator's position, rad; zero without
36    /// `limit_rate` and with [`Backend::Impedance`]. Microradians are the noise of the
37    /// limiter's velocity and acceleration reference, the robot's float32 echo (FCI v10);
38    /// milliradians mean the generator is outrunning the limits.
39    pub backstop_alteration: f64,
40    /// The joint target of the impedance law, rad; zeros with [`Backend::RobotController`].
41    pub q_goal: [f64; 7],
42    /// The torques sent, Nm, clamped to the torque limits; zeros with
43    /// [`Backend::RobotController`].
44    pub tau: [f64; 7],
45    /// The most, rad, the leash pulled any joint of the generator's anchor back from the
46    /// previous goal toward the measured position: zero while the arm follows, positive while
47    /// it is held back ([`Leash`](super::Leash)); 0 with [`Backend::RobotController`].
48    pub leash_alteration: f64,
49}
50
51/// The observer's type: called every cycle on the realtime thread, so it must not allocate
52/// or block.
53pub type JointObserver = Box<dyn FnMut(&RobotState, &JointSent) + Send>;
54
55/// The handle of a running joint target control; see the [module documentation](super).
56pub struct JointTargetControl {
57    inner: Handle<7>,
58    limits: ([f64; 7], [f64; 7]),
59}
60
61impl JointTargetControl {
62    /// Sets the target joint positions, rad. Callable from any thread at any rate; only the
63    /// latest target counts.
64    ///
65    /// # Errors
66    /// [`crate::error::FrankaError::InvalidArgument`] if a value is not finite or outside the
67    /// arm's joint position limits inset by [`JOINT_LIMIT_INSET`](super::JOINT_LIMIT_INSET),
68    /// [`crate::error::FrankaError::InvalidOperation`] with [`super::ENDED_MESSAGE`] once the
69    /// loop has ended for any reason.
70    pub fn set_joints(&self, q: [f64; 7]) -> FrankaResult<()> {
71        if q.iter().all(|v| v.is_finite()) {
72            check_joint_limits(&q, &self.limits, "target")?;
73        }
74        self.inner.set_target(q)
75    }
76
77    /// The latest target set (the start configuration until the first `set_joints`).
78    pub fn target(&self) -> [f64; 7] {
79        self.inner.target()
80    }
81
82    /// The latest robot state the loop received, copied out.
83    pub fn state(&self) -> RobotState {
84        self.inner.state()
85    }
86
87    /// Whether the loop is still running; `false` after it ended for any reason.
88    pub fn is_running(&self) -> bool {
89        self.inner.is_running()
90    }
91
92    /// Settles at the current target, finishes the motion, joins the thread and returns the
93    /// loop's result: `Ok` for a regular end, [`crate::error::FrankaError::Control`] if the
94    /// robot aborted the motion or the deviation guard fired.
95    pub fn stop(self) -> FrankaResult<()> {
96        self.inner.stop()
97    }
98}
99
100pub(super) fn max_abs_difference(a: &[f64; 7], b: &[f64; 7]) -> f64 {
101    a.iter()
102        .zip(b)
103        .map(|(x, y)| (x - y).abs())
104        .fold(0.0, f64::max)
105}
106
107/// The observer's record of a step with `q` as sent; the backend's own fields stay zero.
108pub(super) fn sent(step: &Step<7, 7>, q: [f64; 7]) -> JointSent {
109    JointSent {
110        q,
111        target: step.target,
112        velocity: step.velocity,
113        acceleration: step.acceleration,
114        backstop_alteration: 0.0,
115        q_goal: [0.0; 7],
116        tau: [0.0; 7],
117        leash_alteration: 0.0,
118    }
119}
120
121const THREAD: &str = "franka-joint-target";
122
123pub(super) fn start(
124    robot: &Arc<Robot>,
125    options: JointTargetControlOptions,
126) -> FrankaResult<JointTargetControl> {
127    options.validate()?;
128    let joint_limits = joint_position_limits(robot.fci_version());
129    check_posture(&options.backend, &joint_limits)?;
130    let limits = options.limits.unwrap_or_else(|| {
131        JointTargetControlOptions::scaled_limits(robot.fci_version(), DEFAULT_LIMIT_FRACTION)
132    });
133    let shared = Arc::new(Shared::<7>::default());
134    let loop_shared = Arc::clone(&shared);
135    let priority = options.realtime_priority;
136    let inner = match options.backend {
137        Backend::RobotController => spawn(
138            THREAD,
139            robot,
140            shared,
141            priority,
142            move |robot: &Robot, started| {
143                position_loop(robot, started, options, limits, loop_shared)
144            },
145        )?,
146        Backend::Impedance(impedance) => {
147            let model = Arc::new(robot.load_model()?);
148            spawn(
149                THREAD,
150                robot,
151                shared,
152                priority,
153                move |robot: &Robot, started| {
154                    let limit_rate = options.limit_rate;
155                    torque_loop(options, limits, impedance, model, loop_shared, started)?
156                        .run(robot, limit_rate)
157                },
158            )?
159        }
160    };
161    Ok(JointTargetControl {
162        inner,
163        limits: joint_limits,
164    })
165}
166
167/// The [`Backend::Impedance`] loop of this interface: a [`JointTracker`] on the seven-joint
168/// runner under `limits`.
169pub(super) fn torque_loop(
170    options: JointTargetControlOptions,
171    limits: [OtgLimits; 7],
172    impedance: ImpedanceOptions,
173    model: Arc<Model>,
174    shared: Arc<Shared<7>>,
175    started: SyncSender<()>,
176) -> FrankaResult<TorqueLoop<7, 7, JointTracker>> {
177    let runner = Runner::new(shared, started, limits, options.settle, identity)?;
178    let tracker = JointTracker::new(&options);
179    Ok(TorqueLoop::new(
180        runner,
181        model,
182        impedance,
183        tracker,
184        options.observer,
185    ))
186}
187
188/// [`Backend::RobotController`]: the position stream, re-anchored on the robot's echo every
189/// cycle with the rate limiter as the backstop; a stop holds the echo bit for bit.
190fn position_loop(
191    robot: &Robot,
192    started: SyncSender<()>,
193    options: JointTargetControlOptions,
194    limits: [OtgLimits; 7],
195    shared: Arc<Shared<7>>,
196) -> FrankaResult<()> {
197    let JointTargetControlOptions {
198        controller_mode,
199        max_deviation,
200        settle,
201        limit_rate,
202        mut observer,
203        ..
204    } = options;
205    let mut runner = Runner::new(shared, started, limits, settle, identity)?;
206    let velocity = limits.map(|l| l.max_velocity);
207    let acceleration = limits.map(|l| l.max_acceleration);
208    let jerk = limits.map(|l| l.max_jerk);
209    let mut start: Option<[f64; 7]> = None;
210    let result = robot.control_joint_positions(
211        |state: &RobotState, _period| {
212            let start = *start.get_or_insert(state.q_d);
213            let strayed = max_abs_difference(&state.q, &start) > max_deviation;
214            let step = runner.cycle(state, state.q_d, strayed);
215
216            let mut q = step.position;
217            let mut backstop_alteration = 0.0;
218            if limit_rate && !step.hold {
219                // The budget, tightened to the robot's own velocity envelope at `q`
220                // (position-dependent on an FR3), against the robot's echo; it only fails
221                // on non-finite input, and holding the echo is then the safe command.
222                // Skipped while the stop holds: that command must go out bit-identical.
223                let upper = robot.upper_joint_velocity_limits(&state.q);
224                let lower = robot.lower_joint_velocity_limits(&state.q);
225                let upper: [f64; 7] = std::array::from_fn(|i| upper[i].min(velocity[i]));
226                let lower: [f64; 7] = std::array::from_fn(|i| lower[i].max(-velocity[i]));
227                #[rustfmt::skip]
228                let limited = limit_rate_joint_positions(
229                    &upper, &lower, &acceleration, &jerk,
230                    &q, &state.q_d, &state.dq_d, &state.ddq_d,
231                ).unwrap_or(state.q_d);
232                backstop_alteration = max_abs_difference(&limited, &q);
233                q = limited;
234            }
235            if let Some(observe) = observer.as_mut() {
236                observe(
237                    state,
238                    &JointSent {
239                        backstop_alteration,
240                        ..sent(&step, q)
241                    },
242                );
243            }
244            let mut output = JointPositions::new(q);
245            output.motion_finished = step.finished;
246            output
247        },
248        controller_mode,
249        limit_rate,
250        MAX_CUTOFF_FREQUENCY,
251    );
252    runner.finish(result)
253}
franka/robot/target_control/joint.rs

franka/robot/target_control/
joint.rs
