franka/robot/target_control/
options.rs

1//! The options of the two [target control](super) interfaces and their builders.
2
3use super::cartesian::{CartesianObserver, CartesianSent};
4use super::joint::{JointObserver, JointSent};
5use super::{validate_common, Backend, ImpedanceOptions, Scheduling, Settle};
6use crate::control_types::ControllerMode;
7use crate::error::{FrankaError, FrankaResult};
8use crate::otg::{MultiOtg, OtgLimits};
9use crate::robot::control_loop::rate_limits;
10use crate::robot_state::RobotState;
11use crate::wire::robot::codec::FciVersion;
12
13/// The fraction of the robot's joint limits [`JointTargetControlOptions::default`] budgets.
14pub const DEFAULT_LIMIT_FRACTION: f64 = 0.2;
15
16/// Options of [`Robot::start_cartesian_target_control`](crate::Robot::start_cartesian_target_control);
17/// [`Default`] is the documented starting point and the `with_*` methods change one field each.
18pub struct TargetControlOptions {
19    /// The translational budget as a *norm*: m/s, m/s^2, m/s^3. The generator gets
20    /// [`OtgLimits::per_axis_for_norm`]`(3)` of it and the backstop the norm itself.
21    /// Default 0.3, 0.5, 20: near an FER's ready pose the robot's joint-space continuity check
22    /// refuses 2.5 m/s^2 and its collision threshold trips above about 1 m/s^2, so the default
23    /// sits well below both.
24    pub limits: OtgLimits,
25    /// The rotational budget as a *norm*: rad/s, rad/s^2, rad/s^3; the generator gets
26    /// libfranka's pose-interface factor (0.99) of its `per_axis_for_norm(3)`, the backstop
27    /// applies that factor itself. Default 0.5, 1.0, 20: a fifth of the FR3's velocity
28    /// limit, validated on the simulator's joint-side check only.
29    pub rotation_limits: OtgLimits,
30    /// What turns the generator's pose into a command: the crate's impedance law sending
31    /// torques, or a pose stream to the robot's own controller. Default
32    /// [`Backend::Impedance`] of [`ImpedanceOptions::cartesian`].
33    pub backend: Backend,
34    /// The robot's internal controller, with [`Backend::RobotController`] only. Default
35    /// [`ControllerMode::CartesianImpedance`].
36    pub controller_mode: ControllerMode,
37    /// How far, m, the measured `O_T_EE` position may stray from the start before the target
38    /// is frozen and the loop ends with [`super::DEVIATION_MESSAGE`]. Default 0.30.
39    pub max_deviation: f64,
40    /// How far, rad, the measured `O_T_EE` orientation may turn from the start before the
41    /// same guard fires. Default 0.5.
42    pub max_angular_deviation: f64,
43    /// When a stop counts as settled; the tolerance applies to the position in m and to the
44    /// orientation error in rad alike. Default 1 mm and 1 mrad for 250 cycles.
45    pub settle: Settle,
46    /// Whether `limit_rate_cartesian_pose` runs under the two budgets as the backstop, with
47    /// the loop's own libfranka limiter behind it; with [`Backend::Impedance`], whether the
48    /// torque rate limiter runs. Default `true`.
49    pub limit_rate: bool,
50    /// `SCHED_FIFO` priority for the loop thread; `None` is the highest, as in `Robot::new`.
51    pub realtime_priority: Option<i32>,
52    /// The CPU the loop thread is pinned to after raising its priority
53    /// ([`crate::realtime::pin_current_thread_to_cpu`]); `None`, the default, leaves it to
54    /// the scheduler.
55    pub cpu: Option<usize>,
56    /// Called every cycle on the realtime thread; see [`CartesianObserver`].
57    pub observer: Option<CartesianObserver>,
58}
59
60impl Default for TargetControlOptions {
61    fn default() -> Self {
62        TargetControlOptions {
63            limits: OtgLimits {
64                max_velocity: 0.3,
65                max_acceleration: 0.5,
66                max_jerk: 20.0,
67            },
68            rotation_limits: OtgLimits {
69                max_velocity: 0.5,
70                max_acceleration: 1.0,
71                max_jerk: 20.0,
72            },
73            backend: Backend::Impedance(ImpedanceOptions::cartesian()),
74            controller_mode: ControllerMode::CartesianImpedance,
75            max_deviation: 0.30,
76            max_angular_deviation: 0.5,
77            settle: Settle::default(),
78            limit_rate: true,
79            realtime_priority: None,
80            cpu: None,
81            observer: None,
82        }
83    }
84}
85
86impl std::fmt::Debug for TargetControlOptions {
87    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
88        f.debug_struct("TargetControlOptions")
89            .field("limits", &self.limits)
90            .field("rotation_limits", &self.rotation_limits)
91            .field("backend", &self.backend)
92            .field("controller_mode", &self.controller_mode)
93            .field("max_deviation", &self.max_deviation)
94            .field("max_angular_deviation", &self.max_angular_deviation)
95            .field("settle", &self.settle)
96            .field("limit_rate", &self.limit_rate)
97            .field("realtime_priority", &self.realtime_priority)
98            .field("cpu", &self.cpu)
99            .field("observer", &self.observer.is_some())
100            .finish()
101    }
102}
103
104impl TargetControlOptions {
105    /// Sets the translational norm budget.
106    pub fn with_limits(mut self, limits: OtgLimits) -> Self {
107        self.limits = limits;
108        self
109    }
110
111    /// Sets the rotational norm budget (rad/s, rad/s^2, rad/s^3).
112    pub fn with_rotation_limits(mut self, limits: OtgLimits) -> Self {
113        self.rotation_limits = limits;
114        self
115    }
116
117    /// Sets the backend.
118    pub fn with_backend(mut self, backend: Backend) -> Self {
119        self.backend = backend;
120        self
121    }
122
123    /// Sets the robot's internal controller ([`Backend::RobotController`] only).
124    pub fn with_controller_mode(mut self, mode: ControllerMode) -> Self {
125        self.controller_mode = mode;
126        self
127    }
128
129    /// Sets the deviation guard on the position, m.
130    pub fn with_max_deviation(mut self, metres: f64) -> Self {
131        self.max_deviation = metres;
132        self
133    }
134
135    /// Sets the deviation guard on the orientation, rad.
136    pub fn with_max_angular_deviation(mut self, radians: f64) -> Self {
137        self.max_angular_deviation = radians;
138        self
139    }
140
141    /// Sets the settle criterion.
142    pub fn with_settle(mut self, settle: Settle) -> Self {
143        self.settle = settle;
144        self
145    }
146
147    /// Switches the backstop and the loop's limiter.
148    pub fn with_limit_rate(mut self, limit_rate: bool) -> Self {
149        self.limit_rate = limit_rate;
150        self
151    }
152
153    /// Sets the loop thread's `SCHED_FIFO` priority (`None`: the highest).
154    pub fn with_realtime_priority(mut self, priority: Option<i32>) -> Self {
155        self.realtime_priority = priority;
156        self
157    }
158
159    /// Sets the CPU the loop thread is pinned to (`None`: not pinned).
160    pub fn with_cpu(mut self, cpu: Option<usize>) -> Self {
161        self.cpu = cpu;
162        self
163    }
164
165    /// Installs the observer.
166    pub fn with_observer(
167        mut self,
168        observer: impl FnMut(&RobotState, &CartesianSent) + Send + 'static,
169    ) -> Self {
170        self.observer = Some(Box::new(observer));
171        self
172    }
173
174    /// Checks the options without starting anything.
175    ///
176    /// # Errors
177    /// [`crate::error::FrankaError::InvalidArgument`] naming the field.
178    pub fn validate(&self) -> FrankaResult<()> {
179        crate::otg::Otg::new(0.0, self.limits)?;
180        crate::otg::Otg::new(0.0, self.rotation_limits).map_err(|_| {
181            FrankaError::InvalidArgument(format!(
182                "target control: rotation_limits must be finite and positive, got {:?}",
183                self.rotation_limits
184            ))
185        })?;
186        if !(self.max_angular_deviation.is_finite() && self.max_angular_deviation > 0.0) {
187            return Err(FrankaError::InvalidArgument(format!(
188                "target control: max_angular_deviation must be finite and positive, got {}",
189                self.max_angular_deviation
190            )));
191        }
192        validate_common(
193            self.max_deviation,
194            self.settle,
195            self.scheduling(),
196            &self.backend,
197        )
198    }
199
200    pub(super) fn scheduling(&self) -> Scheduling {
201        Scheduling {
202            priority: self.realtime_priority,
203            cpu: self.cpu,
204        }
205    }
206}
207
208/// Options of [`Robot::start_joint_target_control`](crate::Robot::start_joint_target_control);
209/// [`Default`] is the documented starting point and the `with_*` methods change one field each.
210pub struct JointTargetControlOptions {
211    /// Per-joint limits, rad/s, rad/s^2, rad/s^3, for the generator and, with
212    /// [`Backend::RobotController`], the backstop alike. `None`, the default, is
213    /// [`JointTargetControlOptions::scaled_limits`] of the negotiated FCI version at
214    /// [`DEFAULT_LIMIT_FRACTION`], resolved when the loop starts.
215    pub limits: Option<[OtgLimits; 7]>,
216    /// What turns the generator's joint positions into a command: the crate's impedance law
217    /// sending torques, or a position stream to the robot's own controller. Default
218    /// [`Backend::Impedance`] of [`ImpedanceOptions::joint`].
219    pub backend: Backend,
220    /// The robot's internal controller, with [`Backend::RobotController`] only. Default
221    /// [`ControllerMode::JointImpedance`].
222    pub controller_mode: ControllerMode,
223    /// How far, rad, any measured joint may stray from its start before the target is frozen
224    /// and the loop ends with [`super::DEVIATION_MESSAGE`]. Default 1.0.
225    pub max_deviation: f64,
226    /// When a stop counts as settled. Default 1 mrad for 250 cycles.
227    pub settle: Settle,
228    /// Whether `limit_rate_joint_positions` runs under the limits as the backstop, with the
229    /// loop's own libfranka limiter behind it; with [`Backend::Impedance`], whether the torque
230    /// rate limiter runs. Default `true`.
231    pub limit_rate: bool,
232    /// `SCHED_FIFO` priority for the loop thread; `None` is the highest, as in `Robot::new`.
233    pub realtime_priority: Option<i32>,
234    /// The CPU the loop thread is pinned to after raising its priority
235    /// ([`crate::realtime::pin_current_thread_to_cpu`]); `None`, the default, leaves it to
236    /// the scheduler.
237    pub cpu: Option<usize>,
238    /// Called every cycle on the realtime thread; see [`JointObserver`].
239    pub observer: Option<JointObserver>,
240}
241
242impl Default for JointTargetControlOptions {
243    fn default() -> Self {
244        JointTargetControlOptions {
245            limits: None,
246            backend: Backend::Impedance(ImpedanceOptions::joint()),
247            controller_mode: ControllerMode::JointImpedance,
248            max_deviation: 1.0,
249            settle: Settle::default(),
250            limit_rate: true,
251            realtime_priority: None,
252            cpu: None,
253            observer: None,
254        }
255    }
256}
257
258impl std::fmt::Debug for JointTargetControlOptions {
259    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
260        f.debug_struct("JointTargetControlOptions")
261            .field("limits", &self.limits)
262            .field("backend", &self.backend)
263            .field("controller_mode", &self.controller_mode)
264            .field("max_deviation", &self.max_deviation)
265            .field("settle", &self.settle)
266            .field("limit_rate", &self.limit_rate)
267            .field("realtime_priority", &self.realtime_priority)
268            .field("cpu", &self.cpu)
269            .field("observer", &self.observer.is_some())
270            .finish()
271    }
272}
273
274impl JointTargetControlOptions {
275    /// `fraction` of `version`'s joint limits: [`max_joint_velocity`](super::max_joint_velocity)
276    /// and the version's `MAX_JOINT_ACCELERATION` and `MAX_JOINT_JERK`.
277    pub fn scaled_limits(version: FciVersion, fraction: f64) -> [OtgLimits; 7] {
278        let rate = rate_limits(version);
279        let velocity = super::max_joint_velocity(version);
280        std::array::from_fn(|i| OtgLimits {
281            max_velocity: velocity[i] * fraction,
282            max_acceleration: rate.max_joint_acceleration[i] * fraction,
283            max_jerk: rate.max_joint_jerk[i] * fraction,
284        })
285    }
286
287    /// Sets the per-joint limits.
288    pub fn with_limits(mut self, limits: [OtgLimits; 7]) -> Self {
289        self.limits = Some(limits);
290        self
291    }
292
293    /// Sets the backend.
294    pub fn with_backend(mut self, backend: Backend) -> Self {
295        self.backend = backend;
296        self
297    }
298
299    /// Sets the robot's internal controller ([`Backend::RobotController`] only).
300    pub fn with_controller_mode(mut self, mode: ControllerMode) -> Self {
301        self.controller_mode = mode;
302        self
303    }
304
305    /// Sets the deviation guard, rad.
306    pub fn with_max_deviation(mut self, radians: f64) -> Self {
307        self.max_deviation = radians;
308        self
309    }
310
311    /// Sets the settle criterion.
312    pub fn with_settle(mut self, settle: Settle) -> Self {
313        self.settle = settle;
314        self
315    }
316
317    /// Switches the backstop and the loop's limiter.
318    pub fn with_limit_rate(mut self, limit_rate: bool) -> Self {
319        self.limit_rate = limit_rate;
320        self
321    }
322
323    /// Sets the loop thread's `SCHED_FIFO` priority (`None`: the highest).
324    pub fn with_realtime_priority(mut self, priority: Option<i32>) -> Self {
325        self.realtime_priority = priority;
326        self
327    }
328
329    /// Sets the CPU the loop thread is pinned to (`None`: not pinned).
330    pub fn with_cpu(mut self, cpu: Option<usize>) -> Self {
331        self.cpu = cpu;
332        self
333    }
334
335    /// Installs the observer.
336    pub fn with_observer(
337        mut self,
338        observer: impl FnMut(&RobotState, &JointSent) + Send + 'static,
339    ) -> Self {
340        self.observer = Some(Box::new(observer));
341        self
342    }
343
344    /// Checks the options without starting anything.
345    ///
346    /// # Errors
347    /// [`crate::error::FrankaError::InvalidArgument`] naming the field.
348    pub fn validate(&self) -> FrankaResult<()> {
349        if let Some(limits) = self.limits {
350            MultiOtg::with_limits([0.0; 7], limits, true)?;
351        }
352        validate_common(
353            self.max_deviation,
354            self.settle,
355            self.scheduling(),
356            &self.backend,
357        )
358    }
359
360    pub(super) fn scheduling(&self) -> Scheduling {
361        Scheduling {
362            priority: self.realtime_priority,
363            cpu: self.cpu,
364        }
365    }
366}
franka/robot/target_control/options.rs

franka/robot/target_control/
options.rs
