franka/robot/control.rs
1//! The callback control loops of [`Robot`] (`franka::Robot::control`).
2
3use super::*;
4
5impl Robot {
6 // -- Control loops -------------------------------------------------------------------
7
8 /// Starts a control loop for sending joint-level torque commands.
9 ///
10 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
11 /// low-pass filter applied to the commanded signal (pass
12 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
13 ///
14 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
15 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
16 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
17 /// both are always passed explicitly here.
18 ///
19 /// # Errors
20 /// [`FrankaError::Control`] if an error related to torque control or motion generation
21 /// occurred,
22 /// [`FrankaError::InvalidArgument`] if joint-level torque commands are NaN or infinity,
23 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
24 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
25 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
26 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
27 ///
28 /// # FCI v5
29 /// An FER has no torque-only motion generator mode, so this runs libfranka 0.9.2's scheme
30 /// (`src/robot.cpp:41-57`): a joint-velocity motion generator commanding all-zero
31 /// velocities alongside the external controller, ended with `motion_generation_finished`.
32 /// The two callbacks are also evaluated in 0.9.2's order (motion first, controller
33 /// short-circuited) rather than 0.21.2's.
34 pub fn control_torques<C>(
35 &self,
36 mut control_callback: C,
37 limit_rate: bool,
38 cutoff_frequency: f64,
39 ) -> FrankaResult<()>
40 where
41 C: FnMut(&RobotState, Duration) -> Torques,
42 {
43 let _lock = self.acquire_control_lock()?;
44 control_loop::control_torques(
45 &self.robot,
46 &mut control_callback,
47 limit_rate,
48 cutoff_frequency,
49 )
50 }
51
52 /// Starts a control loop for a joint position motion generator with an external controller.
53 ///
54 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
55 /// low-pass filter applied to the commanded signal (pass
56 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
57 ///
58 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
59 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
60 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
61 /// both are always passed explicitly here.
62 ///
63 /// # Errors
64 /// [`FrankaError::Control`] if an error related to torque control or motion generation
65 /// occurred,
66 /// [`FrankaError::InvalidArgument`] if joint-level torque or joint position commands are
67 /// NaN or infinity,
68 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
69 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
70 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
71 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
72 ///
73 /// # FCI v5
74 /// On an FER the *first* setpoint of the motion is rate limited against the robot's own
75 /// `q_d` / `O_T_EE_c` like every other one, because libfranka 0.9.2's `convertMotion`
76 /// (`src/control_loop.cpp:188-205`) has no `initialized_filter_`. On an FR3 the first
77 /// setpoint is its own reference and therefore passes the limiter unchanged, which is
78 /// libfranka 0.21.2's behaviour (`src/control_loop.cpp:194-200`). Start an FER motion from
79 /// (close to) the current pose either way.
80 pub fn control_torques_and_joint_positions<C, M>(
81 &self,
82 control_callback: C,
83 motion_callback: M,
84 limit_rate: bool,
85 cutoff_frequency: f64,
86 ) -> FrankaResult<()>
87 where
88 C: FnMut(&RobotState, Duration) -> Torques,
89 M: FnMut(&RobotState, Duration) -> JointPositions,
90 {
91 self.control_with_torques(
92 control_callback,
93 motion_callback,
94 limit_rate,
95 cutoff_frequency,
96 )
97 }
98
99 /// Starts a control loop for a joint velocity motion generator with an external controller.
100 ///
101 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
102 /// low-pass filter applied to the commanded signal (pass
103 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
104 ///
105 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
106 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
107 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
108 /// both are always passed explicitly here.
109 ///
110 /// # Errors
111 /// [`FrankaError::Control`] if an error related to torque control or motion generation
112 /// occurred,
113 /// [`FrankaError::InvalidArgument`] if joint-level torque or joint velocity commands are
114 /// NaN or infinity,
115 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
116 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
117 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
118 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
119 pub fn control_torques_and_joint_velocities<C, M>(
120 &self,
121 control_callback: C,
122 motion_callback: M,
123 limit_rate: bool,
124 cutoff_frequency: f64,
125 ) -> FrankaResult<()>
126 where
127 C: FnMut(&RobotState, Duration) -> Torques,
128 M: FnMut(&RobotState, Duration) -> JointVelocities,
129 {
130 self.control_with_torques(
131 control_callback,
132 motion_callback,
133 limit_rate,
134 cutoff_frequency,
135 )
136 }
137
138 /// Starts a control loop for a Cartesian pose motion generator with an external controller.
139 ///
140 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
141 /// low-pass filter applied to the commanded signal (pass
142 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
143 ///
144 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
145 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
146 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
147 /// both are always passed explicitly here.
148 ///
149 /// # Errors
150 /// [`FrankaError::Control`] if an error related to torque control or motion generation
151 /// occurred,
152 /// [`FrankaError::InvalidArgument`] if joint-level torque or Cartesian pose command elements
153 /// are NaN or infinity,
154 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
155 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
156 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
157 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
158 ///
159 /// # FCI v5
160 /// On an FER the *first* setpoint of the motion is rate limited against the robot's own
161 /// `q_d` / `O_T_EE_c` like every other one, because libfranka 0.9.2's `convertMotion`
162 /// (`src/control_loop.cpp:188-205`) has no `initialized_filter_`. On an FR3 the first
163 /// setpoint is its own reference and therefore passes the limiter unchanged, which is
164 /// libfranka 0.21.2's behaviour (`src/control_loop.cpp:194-200`). Start an FER motion from
165 /// (close to) the current pose either way.
166 pub fn control_torques_and_cartesian_pose<C, M>(
167 &self,
168 control_callback: C,
169 motion_callback: M,
170 limit_rate: bool,
171 cutoff_frequency: f64,
172 ) -> FrankaResult<()>
173 where
174 C: FnMut(&RobotState, Duration) -> Torques,
175 M: FnMut(&RobotState, Duration) -> CartesianPose,
176 {
177 self.control_with_torques(
178 control_callback,
179 motion_callback,
180 limit_rate,
181 cutoff_frequency,
182 )
183 }
184
185 /// Starts a control loop for a Cartesian velocity motion generator with an external
186 /// controller.
187 ///
188 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
189 /// low-pass filter applied to the commanded signal (pass
190 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
191 ///
192 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
193 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
194 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
195 /// both are always passed explicitly here.
196 ///
197 /// # Errors
198 /// [`FrankaError::Control`] if an error related to torque control or motion generation
199 /// occurred,
200 /// [`FrankaError::InvalidArgument`] if joint-level torque or Cartesian velocity command
201 /// elements are NaN or infinity,
202 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
203 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
204 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
205 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
206 pub fn control_torques_and_cartesian_velocities<C, M>(
207 &self,
208 control_callback: C,
209 motion_callback: M,
210 limit_rate: bool,
211 cutoff_frequency: f64,
212 ) -> FrankaResult<()>
213 where
214 C: FnMut(&RobotState, Duration) -> Torques,
215 M: FnMut(&RobotState, Duration) -> CartesianVelocities,
216 {
217 self.control_with_torques(
218 control_callback,
219 motion_callback,
220 limit_rate,
221 cutoff_frequency,
222 )
223 }
224
225 /// Starts a control loop for a joint position motion generator with one of the robot's
226 /// internal controllers.
227 ///
228 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
229 /// low-pass filter applied to the commanded signal (pass
230 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
231 ///
232 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
233 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
234 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
235 /// both are always passed explicitly here.
236 ///
237 /// # Errors
238 /// [`FrankaError::Control`] if an error related to motion generation occurred,
239 /// [`FrankaError::InvalidArgument`] if joint position commands are NaN or infinity,
240 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
241 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
242 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
243 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
244 ///
245 /// # FCI v5
246 /// On an FER the *first* setpoint of the motion is rate limited against the robot's own
247 /// `q_d` / `O_T_EE_c` like every other one, because libfranka 0.9.2's `convertMotion`
248 /// (`src/control_loop.cpp:188-205`) has no `initialized_filter_`. On an FR3 the first
249 /// setpoint is its own reference and therefore passes the limiter unchanged, which is
250 /// libfranka 0.21.2's behaviour (`src/control_loop.cpp:194-200`). Start an FER motion from
251 /// (close to) the current pose either way.
252 pub fn control_joint_positions<M>(
253 &self,
254 motion_callback: M,
255 controller_mode: ControllerMode,
256 limit_rate: bool,
257 cutoff_frequency: f64,
258 ) -> FrankaResult<()>
259 where
260 M: FnMut(&RobotState, Duration) -> JointPositions,
261 {
262 self.control_with_mode(
263 motion_callback,
264 controller_mode,
265 limit_rate,
266 cutoff_frequency,
267 )
268 }
269
270 /// Starts a control loop for a joint velocity motion generator with one of the robot's
271 /// internal controllers.
272 ///
273 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
274 /// low-pass filter applied to the commanded signal (pass
275 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
276 ///
277 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
278 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
279 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
280 /// both are always passed explicitly here.
281 ///
282 /// # Errors
283 /// [`FrankaError::Control`] if an error related to motion generation occurred,
284 /// [`FrankaError::InvalidArgument`] if joint velocity commands are NaN or infinity,
285 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
286 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
287 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
288 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
289 pub fn control_joint_velocities<M>(
290 &self,
291 motion_callback: M,
292 controller_mode: ControllerMode,
293 limit_rate: bool,
294 cutoff_frequency: f64,
295 ) -> FrankaResult<()>
296 where
297 M: FnMut(&RobotState, Duration) -> JointVelocities,
298 {
299 self.control_with_mode(
300 motion_callback,
301 controller_mode,
302 limit_rate,
303 cutoff_frequency,
304 )
305 }
306
307 /// Starts a control loop for a Cartesian pose motion generator with one of the robot's
308 /// internal controllers.
309 ///
310 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
311 /// low-pass filter applied to the commanded signal (pass
312 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
313 ///
314 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
315 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
316 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
317 /// both are always passed explicitly here.
318 ///
319 /// # Errors
320 /// [`FrankaError::Control`] if an error related to motion generation occurred,
321 /// [`FrankaError::InvalidArgument`] if Cartesian pose command elements are NaN or infinity,
322 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
323 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
324 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
325 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
326 ///
327 /// # FCI v5
328 /// On an FER the *first* setpoint of the motion is rate limited against the robot's own
329 /// `q_d` / `O_T_EE_c` like every other one, because libfranka 0.9.2's `convertMotion`
330 /// (`src/control_loop.cpp:188-205`) has no `initialized_filter_`. On an FR3 the first
331 /// setpoint is its own reference and therefore passes the limiter unchanged, which is
332 /// libfranka 0.21.2's behaviour (`src/control_loop.cpp:194-200`). Start an FER motion from
333 /// (close to) the current pose either way.
334 pub fn control_cartesian_pose<M>(
335 &self,
336 motion_callback: M,
337 controller_mode: ControllerMode,
338 limit_rate: bool,
339 cutoff_frequency: f64,
340 ) -> FrankaResult<()>
341 where
342 M: FnMut(&RobotState, Duration) -> CartesianPose,
343 {
344 self.control_with_mode(
345 motion_callback,
346 controller_mode,
347 limit_rate,
348 cutoff_frequency,
349 )
350 }
351
352 /// Starts a control loop for a Cartesian velocity motion generator with one of the robot's
353 /// internal controllers.
354 ///
355 /// `limit_rate` enables the client-side rate limiter and `cutoff_frequency` the first-order
356 /// low-pass filter applied to the commanded signal (pass
357 /// [`crate::lowpass_filter::MAX_CUTOFF_FREQUENCY`] to disable the filter).
358 ///
359 /// libfranka 0.21 default: `false` (0.9: `true`); pass `true` to enable the client-side
360 /// limiter -- it could distort your motion. libfranka's `cutoff_frequency` default is
361 /// [`crate::lowpass_filter::DEFAULT_CUTOFF_FREQUENCY`]. Rust has no default arguments, so
362 /// both are always passed explicitly here.
363 ///
364 /// # Errors
365 /// [`FrankaError::Control`] if an error related to motion generation occurred,
366 /// [`FrankaError::InvalidArgument`] if Cartesian velocity command elements are NaN or infinity,
367 /// [`FrankaError::InvalidOperation`] if another control or read operation is running,
368 /// [`FrankaError::Network`] if the connection is lost, e.g. after a timeout. Unlike
369 /// libfranka's `control` this never fails with [`FrankaError::Realtime`]: realtime priority
370 /// is raised in [`Robot::new`], matching libfranka's `Robot::Impl` constructor.
371 pub fn control_cartesian_velocities<M>(
372 &self,
373 motion_callback: M,
374 controller_mode: ControllerMode,
375 limit_rate: bool,
376 cutoff_frequency: f64,
377 ) -> FrankaResult<()>
378 where
379 M: FnMut(&RobotState, Duration) -> CartesianVelocities,
380 {
381 self.control_with_mode(
382 motion_callback,
383 controller_mode,
384 limit_rate,
385 cutoff_frequency,
386 )
387 }
388
389 fn control_with_torques<C, M, T>(
390 &self,
391 mut control_callback: C,
392 mut motion_callback: M,
393 limit_rate: bool,
394 cutoff_frequency: f64,
395 ) -> FrankaResult<()>
396 where
397 C: FnMut(&RobotState, Duration) -> Torques,
398 M: FnMut(&RobotState, Duration) -> T,
399 T: ControlLoopMotion,
400 {
401 let _lock = self.acquire_control_lock()?;
402 ControlLoop::new_with_control_and_motion(
403 &self.robot,
404 &mut control_callback,
405 &mut motion_callback,
406 limit_rate,
407 cutoff_frequency,
408 )?
409 .run()
410 }
411
412 fn control_with_mode<M, T>(
413 &self,
414 mut motion_callback: M,
415 controller_mode: ControllerMode,
416 limit_rate: bool,
417 cutoff_frequency: f64,
418 ) -> FrankaResult<()>
419 where
420 M: FnMut(&RobotState, Duration) -> T,
421 T: ControlLoopMotion,
422 {
423 let _lock = self.acquire_control_lock()?;
424 ControlLoop::new_with_motion(
425 &self.robot,
426 controller_mode,
427 &mut motion_callback,
428 limit_rate,
429 cutoff_frequency,
430 )?
431 .run()
432 }
433}