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

franka/robot/target_control/
options.rs
