franka/robot/robot_impl/
motion.rs

1//! The motion lifecycle of [`RobotImpl`]: `Move`, one control cycle, `StopMove`, the
2//! cancel path and the `ActiveControl` single-command writes.
3
4use super::*;
5
6impl RobotImpl {
7    /// Sends one robot command (`Robot::Impl::sendRobotCommand`).
8    ///
9    /// Returns the command that went out, which is the all-zero command when both arguments are
10    /// `None` — libfranka sends nothing in that case but still logs the empty command. The
11    /// bytes are produced by [`codec::robot_command`], so exactly
12    /// [`codec::command_size`] bytes leave the socket: 370 on FCI v5, 371 on FCI v10.
13    fn send_robot_command(
14        &self,
15        motion_command: Option<&MotionGeneratorCommand>,
16        control_command: Option<&ControllerCommand>,
17    ) -> FrankaResult<RobotCommandData> {
18        let mut data = RobotCommandData::default();
19        if motion_command.is_none() && control_command.is_none() {
20            return Ok(data);
21        }
22
23        let (current_motion_mode, current_controller_mode) = {
24            let guard = self.lock();
25            data.message_id = guard.message_id;
26            (
27                guard.current_move_motion_generator_mode,
28                guard.current_move_controller_mode,
29            )
30        };
31
32        if let Some(motion) = motion_command {
33            if current_motion_mode == StateMotionGeneratorMode::Idle
34                || current_motion_mode == StateMotionGeneratorMode::None
35            {
36                return Err(control_error(
37                    "libfranka robot: Trying to send motion command, but no motion generator \
38                     running!",
39                ));
40            }
41            data.q_c = f64s_to_f64(&motion.q_c);
42            data.dq_c = f64s_to_f64(&motion.dq_c);
43            data.O_T_EE_c = f64s_to_f64(&motion.O_T_EE_c);
44            data.O_dP_EE_c = f64s_to_f64(&motion.O_dP_EE_c);
45            data.elbow_c = f64s_to_f64(&motion.elbow_c);
46            data.valid_elbow = motion.valid_elbow != 0;
47            data.motion_generation_finished = motion.motion_generation_finished != 0;
48        }
49
50        if let Some(control) = control_command {
51            if current_controller_mode != StateControllerMode::ExternalController {
52                return Err(control_error(
53                    "libfranka robot: Trying to send control command, but no controller running!",
54                ));
55            }
56            data.tau_J_d = f64s_to_f64(&control.tau_J_d);
57            data.torque_command_finished = control.torque_command_finished != 0;
58        }
59
60        if current_motion_mode != StateMotionGeneratorMode::Idle
61            && current_motion_mode != StateMotionGeneratorMode::None
62            && current_controller_mode == StateControllerMode::ExternalController
63            && (motion_command.is_none() || control_command.is_none())
64        {
65            return Err(control_error(
66                "libfranka robot: Trying to send partial robot command!",
67            ));
68        }
69
70        let mut bytes = [0u8; codec::ROBOT_COMMAND_MAX_LEN];
71        let length = codec::robot_command(self.version, &data, &mut bytes);
72        debug_assert_eq!(length, codec::command_size(self.version));
73        self.network.send_udp(&bytes[..length])?;
74        Ok(data)
75    }
76
77    /// One control cycle: send the command, wait for the next state, log both
78    /// (`Robot::Impl::updateMotion`).
79    pub fn update_motion(
80        &self,
81        motion_command: Option<&MotionGeneratorCommand>,
82        control_command: Option<&ControllerCommand>,
83    ) -> FrankaResult<RobotState> {
84        self.network.tcp.throw_if_connection_closed()?;
85
86        let command = self.send_robot_command(motion_command, control_command)?;
87        let state = self.receive_robot_state()?;
88        self.logger().log(&state, &command_log(&command));
89
90        Ok(state)
91    }
92
93    /// Waits for the next state without sending a command (`Robot::Impl::readOnce`).
94    pub fn read_once(&self) -> FrankaResult<RobotState> {
95        self.receive_robot_state()
96    }
97
98    /// Detects a motion error and turns the `Move` reply into a [`ControlException`]
99    /// (`Robot::Impl::throwOnMotionError`).
100    pub fn throw_on_motion_error(
101        &self,
102        robot_state: &RobotState,
103        motion_id: u32,
104    ) -> FrankaResult<()> {
105        let errored = {
106            let guard = self.lock();
107            robot_state.robot_mode != RobotMode::Move || !guard.move_active()
108        };
109        if !errored {
110            return Ok(());
111        }
112
113        let message = self.network.tcp.blocking_receive_response(motion_id)?;
114        let status = self.move_status(&message)?;
115        match handle_terminal_move_response(self, status) {
116            Err(FrankaError::Command(text)) => {
117                Err(self.create_control_exception(&text, status, robot_state.last_motion_errors))
118            }
119            Err(other) => Err(other),
120            Ok(()) => Err(FrankaError::Protocol(
121                "Unexpected reply to a Move command".to_string(),
122            )),
123        }
124    }
125
126    /// Starts a motion (`Robot::Impl::startMotion`).
127    ///
128    /// Sends `Move` — 113 bytes on FCI v10 (with `use_async_motion_generator = false` and zero
129    /// maximum velocities), 56 on FCI v5, which has neither field — waits for `MotionStarted`,
130    /// then spins the control cycle until the state reports the requested modes, polling for an
131    /// early terminal `Move` reply on the way.
132    ///
133    /// # Errors
134    /// [`FrankaError::InvalidArgument`] when `motion_generator_mode` is
135    /// [`MoveMotionGeneratorMode::None`] on FCI v5, which has no torque-only mode. Torque
136    /// control on an FER runs a joint-velocity generator commanding zero velocities instead;
137    /// `RobotImpl::torque_only_motion_mode` picks the right mode for the caller.
138    pub fn start_motion(
139        &self,
140        controller_mode: MoveControllerMode,
141        motion_generator_mode: MoveMotionGeneratorMode,
142        maximum_path_deviation: Deviation,
143        maximum_goal_pose_deviation: Deviation,
144    ) -> FrankaResult<u32> {
145        let mut request = [0u8; codec::MOVE_REQUEST_MAX_LEN];
146        let length = codec::move_request(
147            self.version,
148            controller_mode,
149            motion_generator_mode,
150            maximum_path_deviation,
151            maximum_goal_pose_deviation,
152            &mut request,
153        )?;
154        let command = self.command_id(CommandName::Move)?;
155
156        {
157            let mut guard = self.lock();
158            if guard.motion_generator_running() || guard.controller_running() {
159                return Err(control_error(
160                    "libfranka robot: Attempted to start multiple motions!",
161                ));
162            }
163            guard.current_move_motion_generator_mode = match motion_generator_mode {
164                MoveMotionGeneratorMode::JointPosition => StateMotionGeneratorMode::JointPosition,
165                MoveMotionGeneratorMode::JointVelocity => StateMotionGeneratorMode::JointVelocity,
166                MoveMotionGeneratorMode::CartesianPosition => {
167                    StateMotionGeneratorMode::CartesianPosition
168                }
169                MoveMotionGeneratorMode::CartesianVelocity => {
170                    StateMotionGeneratorMode::CartesianVelocity
171                }
172                MoveMotionGeneratorMode::None => StateMotionGeneratorMode::None,
173            };
174            guard.current_move_controller_mode = match controller_mode {
175                MoveControllerMode::JointImpedance => StateControllerMode::JointImpedance,
176                MoveControllerMode::CartesianImpedance => StateControllerMode::CartesianImpedance,
177                MoveControllerMode::ExternalController => StateControllerMode::ExternalController,
178            };
179        }
180
181        let move_command_id = self.network.tcp.send_request(command, &request[..length])?;
182        let response = self
183            .network
184            .tcp
185            .blocking_receive_response(move_command_id)?;
186        handle_move_response(self, self.move_status(&response)?)?;
187
188        while !self.lock().move_active() {
189            if let Some(message) = self.network.tcp.try_receive_response(move_command_id)? {
190                match handle_move_response(self, self.move_status(&message)?) {
191                    Ok(()) => break,
192                    Err(FrankaError::Command(text)) => {
193                        return Err(FrankaError::Control(ControlException::new(text)))
194                    }
195                    Err(other) => return Err(other),
196                }
197            }
198            self.update_motion(None, None)?;
199        }
200
201        self.logger().flush();
202        Ok(move_command_id)
203    }
204
205    /// The `Move` motion generator mode a torque-only control process runs with.
206    ///
207    /// FCI v10 has `kNone` (`Robot::control(control_callback)` in libfranka 0.21.2 starts the
208    /// motion with it). FCI v5 does not: libfranka 0.9.2's `Robot::control(control_callback,
209    /// ...)` (`src/robot.cpp:41-57`) instantiates `ControlLoop<JointVelocities>` with a motion
210    /// callback returning all-zero velocities, i.e. it runs a *joint velocity* generator
211    /// alongside the external controller.
212    pub(crate) fn torque_only_motion_mode(&self) -> MoveMotionGeneratorMode {
213        match self.version {
214            FciVersion::V5 => MoveMotionGeneratorMode::JointVelocity,
215            FciVersion::V10 => MoveMotionGeneratorMode::None,
216        }
217    }
218
219    /// Sends one torque command of a torque-only control process, adding the zero-velocity
220    /// motion command FCI v5 needs alongside it.
221    pub(crate) fn write_once_torque_only(&self, control: &ControllerCommand) -> FrankaResult<()> {
222        match self.version {
223            FciVersion::V5 => {
224                let motion = MotionGeneratorCommand::default();
225                self.write_once_motion_and_control(&motion, control)
226            }
227            FciVersion::V10 => self.write_once_control(control),
228        }
229    }
230
231    /// Ends a torque-only control process, following the same version rule as
232    /// [`RobotImpl::write_once_torque_only`].
233    pub(crate) fn finish_torque_only(
234        &self,
235        motion_id: u32,
236        control: &ControllerCommand,
237    ) -> FrankaResult<()> {
238        match self.version {
239            FciVersion::V5 => {
240                let motion = MotionGeneratorCommand::default();
241                self.finish_motion(motion_id, Some(&motion), Some(control))
242            }
243            FciVersion::V10 => self.finish_motion(motion_id, None, Some(control)),
244        }
245    }
246
247    /// Ends a motion regularly (`Robot::Impl::finishMotion`).
248    ///
249    /// The last command is repeated with the "finished" flag set until the robot leaves the
250    /// motion, then the terminal `Move` reply is claimed.
251    pub fn finish_motion(
252        &self,
253        motion_id: u32,
254        motion_command: Option<&MotionGeneratorCommand>,
255        control_command: Option<&ControllerCommand>,
256    ) -> FrankaResult<()> {
257        {
258            let mut guard = self.lock();
259            if !guard.motion_generator_running() && !guard.controller_running() {
260                guard.current_move_motion_generator_mode = StateMotionGeneratorMode::Idle;
261                guard.current_move_controller_mode = StateControllerMode::Other;
262                return Ok(());
263            }
264        }
265
266        let mut motion_finished_command = motion_command.copied();
267        let mut controller_finished_command = control_command.copied();
268        if let Some(motion) = motion_finished_command.as_mut() {
269            motion.motion_generation_finished = 1;
270        } else if self.version == FciVersion::V5 {
271            // FCI v5's `ControllerCommand` has no `torque_command_finished`, so 0.9.2's
272            // `finishMotion` (`src/robot_impl.cpp:150-155`) insists on a motion command.
273            const MESSAGE: &str = "libfranka robot: No motion generator command given!";
274            log_error(MESSAGE);
275            return Err(control_error(MESSAGE));
276        } else if let Some(control) = controller_finished_command.as_mut() {
277            control.torque_command_finished = 1;
278        } else {
279            const MESSAGE: &str = "libfranka robot: No motion generator or control command given!";
280            log_error(MESSAGE);
281            return Err(control_error(MESSAGE));
282        }
283
284        // The TCP response for the finished Move might arrive while the robot state still shows
285        // that the motion is running, or afterwards. To handle both situations, we do not
286        // process TCP packages in this loop and explicitly wait for the Move response over TCP
287        // afterwards.
288        let mut robot_state = RobotState::default();
289        loop {
290            {
291                let guard = self.lock();
292                if !guard.motion_generator_running() && !guard.controller_running() {
293                    break;
294                }
295            }
296            robot_state = self.update_motion(
297                motion_finished_command.as_ref(),
298                controller_finished_command.as_ref(),
299            )?;
300        }
301
302        let response = self.network.tcp.blocking_receive_response(motion_id)?;
303        let status = self.move_status(&response)?;
304        if status == MoveStatus::ReflexAborted {
305            return Err(self.create_control_exception(
306                "Motion finished commanded, but the robot is still moving!",
307                status,
308                robot_state.last_motion_errors,
309            ));
310        }
311        // The terminal handler, so that `Ok(())` out of a control loop means the motion really
312        // ended with `MoveStatus::Success` (see `handle_terminal_move_response`).
313        match handle_terminal_move_response(self, status) {
314            Ok(()) => {}
315            Err(FrankaError::Command(text)) => {
316                return Err(self.create_control_exception(
317                    &text,
318                    status,
319                    robot_state.last_motion_errors,
320                ))
321            }
322            Err(other) => return Err(other),
323        }
324
325        let mut guard = self.lock();
326        guard.current_move_motion_generator_mode = StateMotionGeneratorMode::Idle;
327        guard.current_move_controller_mode = StateControllerMode::Other;
328        Ok(())
329    }
330
331    /// Aborts a motion (`Robot::Impl::cancelMotion`).
332    ///
333    /// Sends exactly one `StopMove`, drains the state stream until the robot is idle again and
334    /// discards the `Move` reply if it has already arrived.
335    pub fn cancel_motion(&self, motion_id: u32) -> FrankaResult<()> {
336        if !self.network.tcp.is_alive() {
337            log_warn("libfranka robot: TCP connection is closed. Cannot cancel motion.");
338            return Ok(());
339        }
340
341        let stop_command_id = self
342            .network
343            .tcp
344            .send_request(self.command_id(CommandName::StopMove)?, &[])?;
345        let response = self
346            .network
347            .tcp
348            .blocking_receive_response(stop_command_id)?;
349        match handle_stop_move_response(self, self.stop_move_status(&response)?) {
350            Ok(()) => {}
351            Err(FrankaError::Command(text)) => {
352                return Err(FrankaError::Control(ControlException::new(text)))
353            }
354            Err(other) => return Err(other),
355        }
356
357        loop {
358            self.receive_robot_state()?;
359            let guard = self.lock();
360            if !guard.motion_generator_running() && !guard.controller_running() {
361                break;
362            }
363        }
364
365        // Ignore the Move response; it is not guaranteed to have arrived yet.
366        let _ = self.network.tcp.try_receive_response(motion_id)?;
367
368        let mut guard = self.lock();
369        guard.current_move_motion_generator_mode = StateMotionGeneratorMode::Idle;
370        guard.current_move_controller_mode = StateControllerMode::Other;
371        Ok(())
372    }
373
374    /// Sends one torque command outside a control loop (`Robot::Impl::writeOnce(const
375    /// Torques&)`).
376    pub(crate) fn write_once_control(&self, control: &ControllerCommand) -> FrankaResult<()> {
377        self.send_robot_command(None, Some(control))?;
378        Ok(())
379    }
380
381    /// Sends one motion command outside a control loop
382    /// (`Robot::Impl::writeOnce(const MotionGeneratorType&)`).
383    pub(crate) fn write_once_motion(&self, motion: &MotionGeneratorCommand) -> FrankaResult<()> {
384        self.send_robot_command(Some(motion), None)?;
385        Ok(())
386    }
387
388    /// Sends one motion and one torque command outside a control loop
389    /// (`Robot::Impl::writeOnce(const MotionGeneratorType&, const Torques&)`).
390    pub(crate) fn write_once_motion_and_control(
391        &self,
392        motion: &MotionGeneratorCommand,
393        control: &ControllerCommand,
394    ) -> FrankaResult<()> {
395        self.send_robot_command(Some(motion), Some(control))?;
396        Ok(())
397    }
398
399    /// Whether a motion generator is active according to the last state
400    /// (`Robot::Impl::motionGeneratorRunning`).
401    pub fn motion_generator_running(&self) -> bool {
402        self.lock().motion_generator_running()
403    }
404
405    /// Whether an external controller is active according to the last state
406    /// (`Robot::Impl::controllerRunning`).
407    pub fn controller_running(&self) -> bool {
408        self.lock().controller_running()
409    }
410
411    /// Reads the status byte of a `Move` response and maps it through the codec.
412    pub(crate) fn move_status(&self, message: &[u8]) -> FrankaResult<MoveStatus> {
413        codec::parse_move_status(
414            self.version,
415            status_byte(message)?,
416            CommandKind::Move.name(),
417        )
418    }
419
420    /// Reads the status byte of a `StopMove` response and maps it through the codec.
421    pub(crate) fn stop_move_status(
422        &self,
423        message: &[u8],
424    ) -> FrankaResult<crate::wire::robot::StopMoveStatus> {
425        codec::parse_stop_move_status(
426            self.version,
427            status_byte(message)?,
428            CommandKind::StopMove.name(),
429        )
430    }
431
432    /// Builds the [`ControlException`] libfranka's `createControlException` produces, including
433    /// the reflex error names and the estimated command success rate.
434    fn create_control_exception(
435        &self,
436        message: &str,
437        move_status: MoveStatus,
438        reflex_errors: Errors,
439    ) -> FrankaError {
440        let log = self.logger().flush();
441        let exception = create_control_exception(message, move_status, reflex_errors, log);
442        log_error(&exception.message);
443        FrankaError::Control(exception)
444    }
445}
franka/robot/robot_impl/motion.rs

franka/robot/robot_impl/
motion.rs
