franka/robot/
commands.rs

1//! The TCP commands of the robot server and libfranka's response handling.
2//!
3//! Port of `Robot::Impl::executeCommand` and the `handleCommandResponse` overloads in
4//! `src/robot_impl.h` (libfranka 0.21.2). Every rejection text — including
5//! `commandNotPossibleMsg`, which names the current robot mode and adds
6//! `" Did you open the brakes?"` when it is `Other` — is reproduced verbatim.
7
8use zerocopy::{FromBytes, IntoBytes};
9
10use crate::error::{FrankaError, FrankaResult, MoveStatus};
11use crate::robot::robot_impl::{status_byte, RobotImpl};
12use crate::robot::VirtualWallCuboid;
13use crate::wire::robot::codec::{self, CommandKind, FciVersion};
14use crate::wire::robot::v5::{
15    GetCartesianLimitRequest, GetCartesianLimitResponse, SetFiltersRequest,
16};
17use crate::wire::robot::{
18    AutomaticErrorRecoveryStatus, CommandStatus, GetterSetterStatus, SetCartesianImpedanceRequest,
19    SetCollisionBehaviorRequest, SetEEToKRequest, SetGuidingModeRequest, SetJointImpedanceRequest,
20    SetLoadRequest, SetNEToEERequest, StopMoveStatus,
21};
22use crate::wire::{f64s_to_f64, message_payload, HeaderLayout};
23
24/// The command names libfranka puts into its error texts
25/// (`research_interface::robot::CommandTraits<T>::kName`).
26#[derive(Debug, Clone, Copy, PartialEq, Eq)]
27pub enum CommandName {
28    /// Fetches the robot's URDF (`"Get Robot Model"`). FCI v10 only.
29    GetRobotModel,
30    /// Starts a motion (`"Move"`).
31    Move,
32    /// Stops the running motion (`"Stop Move"`), the command behind
33    /// [`crate::Robot::stop`].
34    StopMove,
35    /// Reads a virtual wall cuboid (`"Get Cartesian Limit"`). FCI v5 only
36    /// (`Robot::getVirtualWall`).
37    GetCartesianLimit,
38    /// Port of `franka::Robot::setCollisionBehavior` (`"Set Collision Behavior"`).
39    SetCollisionBehavior,
40    /// Port of `franka::Robot::setJointImpedance` (`"Set Joint Impedance"`).
41    SetJointImpedance,
42    /// Port of `franka::Robot::setCartesianImpedance` (`"Set Cartesian Impedance"`).
43    SetCartesianImpedance,
44    /// Port of `franka::Robot::setGuidingMode` (`"Set Guiding Mode"`).
45    SetGuidingMode,
46    /// Port of `franka::Robot::setK` (`"Set EE To K"`).
47    SetEEToK,
48    /// Port of `franka::Robot::setEE` (`"Set NE To EE"`).
49    SetNEToEE,
50    /// Port of `franka::Robot::setLoad` (`"Set Load"`).
51    SetLoad,
52    /// Port of `franka::Robot::setFilters` (`"Set Filters"`). FCI v5 only.
53    SetFilters,
54    /// Clears a reflex and re-enables motion (`"Automatic Error Recovery"`), the command behind
55    /// [`crate::Robot::automatic_error_recovery`].
56    AutomaticErrorRecovery,
57}
58
59impl CommandName {
60    /// The codec's version-agnostic command identity, which carries the wire numbering of both
61    /// versions ([`codec::command_id`]) and libfranka's `CommandTraits<T>::kName`.
62    pub(crate) const fn kind(self) -> CommandKind {
63        match self {
64            CommandName::GetRobotModel => CommandKind::GetRobotModel,
65            CommandName::Move => CommandKind::Move,
66            CommandName::StopMove => CommandKind::StopMove,
67            CommandName::GetCartesianLimit => CommandKind::GetCartesianLimit,
68            CommandName::SetCollisionBehavior => CommandKind::SetCollisionBehavior,
69            CommandName::SetJointImpedance => CommandKind::SetJointImpedance,
70            CommandName::SetCartesianImpedance => CommandKind::SetCartesianImpedance,
71            CommandName::SetGuidingMode => CommandKind::SetGuidingMode,
72            CommandName::SetEEToK => CommandKind::SetEEToK,
73            CommandName::SetNEToEE => CommandKind::SetNEToEE,
74            CommandName::SetLoad => CommandKind::SetLoad,
75            CommandName::SetFilters => CommandKind::SetFilters,
76            CommandName::AutomaticErrorRecovery => CommandKind::AutomaticErrorRecovery,
77        }
78    }
79
80    /// The name used in error messages.
81    pub const fn as_str(self) -> &'static str {
82        self.kind().name()
83    }
84
85    /// The wire command id of this command under `version`, or `None` when that FCI version
86    /// does not have the command (`Get Robot Model` on an FER, `Set Filters` and
87    /// `Get Cartesian Limit` on an FR3).
88    ///
89    /// This replaces the version-less `command()` of the FR3-only releases: the two protocol
90    /// versions number their commands differently from `SetCollisionBehavior` onwards, so a
91    /// wire id is only meaningful together with a version. `RobotImpl::command_id` is the
92    /// same lookup against a live connection's negotiated version, with the
93    /// [`FrankaError::InvalidOperation`] text libfranka would print.
94    pub const fn command(self, version: FciVersion) -> Option<u32> {
95        codec::command_id(version, self.kind())
96    }
97}
98
99/// Port of `Robot::Impl::commandNotPossibleMsg`.
100fn command_not_possible_message(robot: &RobotImpl) -> String {
101    let mode = robot.robot_mode();
102    let mut message =
103        format!(" command rejected: command not possible in the current mode (\"{mode}\")!");
104    if mode == crate::robot_state::RobotMode::Other {
105        message.push_str(" Did you open the brakes?");
106    }
107    message
108}
109
110/// A `CommandException` with libfranka's `"libfranka: " + kName + <detail>` layout.
111fn command_error(name: CommandName, detail: &str) -> FrankaError {
112    FrankaError::Command(format!("libfranka: {}{detail}", name.as_str()))
113}
114
115/// Port of the `CommandBase` `handleCommandResponse` overload, used by `GetRobotModel`.
116///
117/// `CommandBase::Status` and `GetterSetterCommandBase::Status` agree on `0` and `1` only, so
118/// the two families need separate handlers.
119pub(crate) fn handle_command_response(
120    robot: &RobotImpl,
121    name: CommandName,
122    status: CommandStatus,
123) -> FrankaResult<()> {
124    match status {
125        CommandStatus::Success => Ok(()),
126        CommandStatus::CommandNotPossibleRejected => {
127            Err(command_error(name, &command_not_possible_message(robot)))
128        }
129        CommandStatus::CommandRejectedDueToActivatedSafetyFunctions => Err(command_error(
130            name,
131            " command rejected due to activated safety function! Please disable all safety \
132             functions.",
133        )),
134    }
135}
136
137/// Port of the `IsBaseOfGetterSetter` `handleCommandResponse` overload, shared by every
138/// setter command.
139pub(crate) fn handle_getter_setter_response(
140    robot: &RobotImpl,
141    name: CommandName,
142    status: GetterSetterStatus,
143) -> FrankaResult<()> {
144    match status {
145        GetterSetterStatus::Success => Ok(()),
146        GetterSetterStatus::CommandNotPossibleRejected => {
147            Err(command_error(name, &command_not_possible_message(robot)))
148        }
149        GetterSetterStatus::InvalidArgumentRejected => {
150            Err(command_error(name, " command rejected: invalid argument!"))
151        }
152        GetterSetterStatus::CommandRejectedDueToActivatedSafetyFunctions => Err(command_error(
153            name,
154            " command rejected due to activated safety function! Please disable all safety \
155             functions. ",
156        )),
157    }
158}
159
160/// Port of `handleCommandResponse<research_interface::robot::Move>`.
161///
162/// `kMotionStarted` is accepted only while no motion generator is running, exactly like the
163/// C++ overload (`robot_impl.h:388-395`).
164pub(crate) fn handle_move_response(robot: &RobotImpl, status: MoveStatus) -> FrankaResult<()> {
165    handle_move_status(status, robot.motion_generator_running(), robot)
166}
167
168/// [`handle_move_response`] for the *terminal* `Move` reply of a motion this client started,
169/// i.e. the one claimed by `Robot::Impl::finishMotion` and `Robot::Impl::throwOnMotionError`.
170///
171/// At that point a motion **is** running as far as the client is concerned -- the `motion_id`
172/// belongs to a `Move` that was started and has not been answered yet -- so libfranka's
173/// `motionGeneratorRunning()` guard on `kMotionStarted` is evaluated as `true` here. The C++
174/// code reads the same guard off the *last robot state*, which has already dropped back to
175/// `kIdle` by the time `finishMotion` claims the reply, and therefore silently accepts a second
176/// `kMotionStarted` as if it were `kSuccess`. Diverging here is deliberate and is what makes
177/// `Ok(())` out of `Robot::control_*` mean "the terminal status was `kSuccess`".
178pub(crate) fn handle_terminal_move_response(
179    robot: &RobotImpl,
180    status: MoveStatus,
181) -> FrankaResult<()> {
182    handle_move_status(status, true, robot)
183}
184
185fn handle_move_status(
186    status: MoveStatus,
187    motion_running: bool,
188    robot: &RobotImpl,
189) -> FrankaResult<()> {
190    const NAME: CommandName = CommandName::Move;
191    match status {
192        MoveStatus::Success => Ok(()),
193        MoveStatus::MotionStarted => {
194            if motion_running {
195                return Err(FrankaError::Protocol(
196                    "libfranka: Move received unexpected motion started message.".to_string(),
197                ));
198            }
199            Ok(())
200        }
201        MoveStatus::EmergencyAborted => {
202            Err(command_error(NAME, " command aborted: User Stop pressed!"))
203        }
204        MoveStatus::ReflexAborted => Err(command_error(
205            NAME,
206            " command aborted: motion aborted by reflex!",
207        )),
208        MoveStatus::InputErrorAborted => Err(command_error(
209            NAME,
210            " command aborted: invalid input provided!",
211        )),
212        MoveStatus::CommandNotPossibleRejected => {
213            Err(command_error(NAME, &command_not_possible_message(robot)))
214        }
215        MoveStatus::StartAtSingularPoseRejected => Err(command_error(
216            NAME,
217            " command rejected: cannot start at singular pose!",
218        )),
219        MoveStatus::InvalidArgumentRejected => Err(command_error(
220            NAME,
221            " command rejected: maximum path deviation out of range!",
222        )),
223        MoveStatus::Preempted => Err(command_error(NAME, " command preempted!")),
224        MoveStatus::Aborted => Err(command_error(NAME, " command aborted!")),
225        MoveStatus::PreemptedDueToActivatedSafetyFunctions => Err(command_error(
226            NAME,
227            " command preempted due to activated safety function! Please disable all safety \
228             functions.",
229        )),
230        MoveStatus::CommandRejectedDueToActivatedSafetyFunctions => Err(command_error(
231            NAME,
232            " command rejected due to activated safety function! Please disable all safety \
233             functions.",
234        )),
235    }
236}
237
238/// Port of `handleCommandResponse<research_interface::robot::StopMove>`.
239///
240/// Note that libfranka reports `kAborted` with the "command not possible" text and names the
241/// *`Move`* command in the safety-function case; both quirks are reproduced.
242pub(crate) fn handle_stop_move_response(
243    robot: &RobotImpl,
244    status: StopMoveStatus,
245) -> FrankaResult<()> {
246    const NAME: CommandName = CommandName::StopMove;
247    match status {
248        StopMoveStatus::Success => Ok(()),
249        StopMoveStatus::CommandNotPossibleRejected | StopMoveStatus::Aborted => {
250            Err(command_error(NAME, &command_not_possible_message(robot)))
251        }
252        StopMoveStatus::EmergencyAborted => {
253            Err(command_error(NAME, " command aborted: User Stop pressed!"))
254        }
255        StopMoveStatus::ReflexAborted => Err(command_error(
256            NAME,
257            " command aborted: motion aborted by reflex!",
258        )),
259        StopMoveStatus::CommandRejectedDueToActivatedSafetyFunctions => Err(command_error(
260            CommandName::Move,
261            " command rejected due to activated safety function! Please disable all safety \
262                 functions.",
263        )),
264    }
265}
266
267/// Port of `handleCommandResponse<research_interface::robot::AutomaticErrorRecovery>`.
268pub(crate) fn handle_automatic_error_recovery_response(
269    robot: &RobotImpl,
270    status: AutomaticErrorRecoveryStatus,
271) -> FrankaResult<()> {
272    const NAME: CommandName = CommandName::AutomaticErrorRecovery;
273    match status {
274        AutomaticErrorRecoveryStatus::Success => Ok(()),
275        AutomaticErrorRecoveryStatus::EmergencyAborted => {
276            Err(command_error(NAME, " command aborted: User Stop pressed!"))
277        }
278        AutomaticErrorRecoveryStatus::ReflexAborted => Err(command_error(
279            NAME,
280            " command aborted: motion aborted by reflex!",
281        )),
282        AutomaticErrorRecoveryStatus::CommandNotPossibleRejected => {
283            Err(command_error(NAME, &command_not_possible_message(robot)))
284        }
285        AutomaticErrorRecoveryStatus::ManualErrorRecoveryRequiredRejected => Err(command_error(
286            NAME,
287            " command rejected: manual error recovery required!",
288        )),
289        AutomaticErrorRecoveryStatus::Aborted => Err(command_error(NAME, " command aborted!")),
290        AutomaticErrorRecoveryStatus::CommandRejectedDueToActivatedSafetyFunctions => {
291            Err(command_error(
292                CommandName::Move,
293                " command rejected due to activated safety function! Please disable all safety \
294                 functions.",
295            ))
296        }
297    }
298}
299
300impl RobotImpl {
301    /// Sends a request, blocks for its response and returns the whole message.
302    ///
303    /// The wire command id comes from the negotiated version, so a command that does not exist
304    /// there fails with [`FrankaError::InvalidOperation`] before anything is sent
305    /// ([`RobotImpl::command_id`]).
306    fn execute(&self, name: CommandName, payload: &[u8]) -> FrankaResult<Vec<u8>> {
307        let command_id = self
308            .network()
309            .tcp
310            .send_request(self.command_id(name)?, payload)?;
311        self.network().tcp.blocking_receive_response(command_id)
312    }
313
314    /// `executeCommand<T>` for the getter/setter family.
315    fn execute_setter(&self, name: CommandName, payload: &[u8]) -> FrankaResult<()> {
316        let message = self.execute(name, payload)?;
317        let status = codec::parse_getter_setter_status(
318            self.version(),
319            status_byte(&message)?,
320            name.as_str(),
321        )?;
322        handle_getter_setter_response(self, name, status)
323    }
324
325    /// `GetRobotModel`: returns the robot's URDF.
326    ///
327    /// # Errors
328    /// [`FrankaError::InvalidOperation`] on FCI v5, which has no such command — an FER serves
329    /// its model as a shared object through `LoadModelLibrary` instead.
330    pub fn get_robot_model(&self) -> FrankaResult<String> {
331        const NAME: CommandName = CommandName::GetRobotModel;
332        let message = self.execute(NAME, &[])?;
333        let status =
334            codec::parse_command_status(self.version(), status_byte(&message)?, NAME.as_str())?;
335        handle_command_response(self, NAME, status)?;
336        let payload = message_payload(HeaderLayout::Robot, &message);
337        Ok(String::from_utf8_lossy(&payload[1..]).into_owned())
338    }
339
340    /// `Robot::setFilters` (libfranka 0.9.2 `src/robot.cpp:216-225`), FCI v5 only.
341    ///
342    /// # Errors
343    /// [`FrankaError::InvalidOperation`] on FCI v10, which dropped the command.
344    pub fn set_filters(
345        &self,
346        joint_position_filter_frequency: f64,
347        joint_velocity_filter_frequency: f64,
348        cartesian_position_filter_frequency: f64,
349        cartesian_velocity_filter_frequency: f64,
350        controller_filter_frequency: f64,
351    ) -> FrankaResult<()> {
352        let request = SetFiltersRequest::new(
353            joint_position_filter_frequency,
354            joint_velocity_filter_frequency,
355            cartesian_position_filter_frequency,
356            cartesian_velocity_filter_frequency,
357            controller_filter_frequency,
358        );
359        self.execute_setter(CommandName::SetFilters, request.as_bytes())
360    }
361
362    /// `Robot::getVirtualWall` (libfranka 0.9.2 `src/robot.cpp:227-231`), FCI v5 only.
363    ///
364    /// The 154-byte `GetCartesianLimit::Response` is mapped exactly as the C++
365    /// `executeCommand<GetCartesianLimit>` specialisation does (`src/robot_impl.h:284-300`):
366    /// `p_frame` is the response's `object_frame`, `active` its `object_activation`, and `id`
367    /// is echoed from the request rather than read off the wire.
368    ///
369    /// # Errors
370    /// [`FrankaError::InvalidOperation`] on FCI v10, which dropped the command.
371    pub fn virtual_wall(&self, id: i32) -> FrankaResult<VirtualWallCuboid> {
372        const NAME: CommandName = CommandName::GetCartesianLimit;
373        let request = GetCartesianLimitRequest::new(id);
374        let message = self.execute(NAME, request.as_bytes())?;
375        let payload = message_payload(HeaderLayout::Robot, &message);
376        let response = GetCartesianLimitResponse::read_from_bytes(payload).map_err(|_| {
377            FrankaError::Protocol("libfranka: Incorrect TCP message size.".to_string())
378        })?;
379
380        let wall = VirtualWallCuboid {
381            id,
382            object_world_size: f64s_to_f64(&response.object_world_size),
383            p_frame: f64s_to_f64(&response.object_frame),
384            active: response.object_activation != 0,
385        };
386
387        let status =
388            codec::parse_getter_setter_status(self.version(), response.status, NAME.as_str())?;
389        handle_getter_setter_response(self, NAME, status)?;
390        Ok(wall)
391    }
392
393    /// `Robot::setCollisionBehavior` with all eight threshold arrays.
394    #[allow(clippy::too_many_arguments)]
395    pub fn set_collision_behavior(
396        &self,
397        lower_torque_thresholds_acceleration: &[f64; 7],
398        upper_torque_thresholds_acceleration: &[f64; 7],
399        lower_torque_thresholds_nominal: &[f64; 7],
400        upper_torque_thresholds_nominal: &[f64; 7],
401        lower_force_thresholds_acceleration: &[f64; 6],
402        upper_force_thresholds_acceleration: &[f64; 6],
403        lower_force_thresholds_nominal: &[f64; 6],
404        upper_force_thresholds_nominal: &[f64; 6],
405    ) -> FrankaResult<()> {
406        let request = SetCollisionBehaviorRequest::new(
407            lower_torque_thresholds_acceleration,
408            upper_torque_thresholds_acceleration,
409            lower_torque_thresholds_nominal,
410            upper_torque_thresholds_nominal,
411            lower_force_thresholds_acceleration,
412            upper_force_thresholds_acceleration,
413            lower_force_thresholds_nominal,
414            upper_force_thresholds_nominal,
415        );
416        self.execute_setter(CommandName::SetCollisionBehavior, request.as_bytes())
417    }
418
419    /// `Robot::setJointImpedance`.
420    pub fn set_joint_impedance(&self, K_theta: &[f64; 7]) -> FrankaResult<()> {
421        let request = SetJointImpedanceRequest::new(K_theta);
422        self.execute_setter(CommandName::SetJointImpedance, request.as_bytes())
423    }
424
425    /// `Robot::setCartesianImpedance`.
426    pub fn set_cartesian_impedance(&self, K_x: &[f64; 6]) -> FrankaResult<()> {
427        let request = SetCartesianImpedanceRequest::new(K_x);
428        self.execute_setter(CommandName::SetCartesianImpedance, request.as_bytes())
429    }
430
431    /// `Robot::setGuidingMode`.
432    pub fn set_guiding_mode(&self, guiding_mode: &[bool; 6], elbow: bool) -> FrankaResult<()> {
433        let request = SetGuidingModeRequest::new(guiding_mode, elbow);
434        self.execute_setter(CommandName::SetGuidingMode, request.as_bytes())
435    }
436
437    /// `Robot::setK`.
438    pub fn set_k(&self, EE_T_K: &[f64; 16]) -> FrankaResult<()> {
439        let request = SetEEToKRequest::new(EE_T_K);
440        self.execute_setter(CommandName::SetEEToK, request.as_bytes())
441    }
442
443    /// `Robot::setEE`.
444    pub fn set_ee(&self, NE_T_EE: &[f64; 16]) -> FrankaResult<()> {
445        let request = SetNEToEERequest::new(NE_T_EE);
446        self.execute_setter(CommandName::SetNEToEE, request.as_bytes())
447    }
448
449    /// `Robot::setLoad`.
450    pub fn set_load(
451        &self,
452        load_mass: f64,
453        F_x_Cload: &[f64; 3],
454        load_inertia: &[f64; 9],
455    ) -> FrankaResult<()> {
456        let request = SetLoadRequest::new(load_mass, F_x_Cload, load_inertia);
457        self.execute_setter(CommandName::SetLoad, request.as_bytes())
458    }
459
460    /// `Robot::automaticErrorRecovery`.
461    pub fn automatic_error_recovery(&self) -> FrankaResult<()> {
462        const NAME: CommandName = CommandName::AutomaticErrorRecovery;
463        let message = self.execute(NAME, &[])?;
464        let status = codec::parse_automatic_error_recovery_status(
465            self.version(),
466            status_byte(&message)?,
467            NAME.as_str(),
468        )?;
469        handle_automatic_error_recovery_response(self, status)
470    }
471
472    /// `Robot::stop`: a bare `StopMove`, callable while a control loop is running.
473    pub fn stop(&self) -> FrankaResult<()> {
474        let message = self.execute(CommandName::StopMove, &[])?;
475        handle_stop_move_response(self, self.stop_move_status(&message)?)
476    }
477}
franka/robot/commands.rs

franka/robot/
commands.rs
