franka/wire/robot/codec/
mod.rs

1//! The version seam of the robot protocol.
2//!
3//! Everything that differs between FCI v5 (Franka Emika Robot, FER, libfranka 0.9.2) and FCI v10
4//! (Franka Research 3, libfranka 0.21.2) at the byte level is funnelled through the functions
5//! in this module, so that [`crate::robot`] holds a single [`FciVersion`] and never names a
6//! version-specific wire type.
7//!
8//! The functions take a `&mut [u8; N]` output buffer sized for the **larger** of the two
9//! versions and return how many bytes they wrote; there is no allocation and no `dyn`
10//! dispatch, only a `match` on [`FciVersion`], so they are safe to call from a control cycle.
11//!
12//! Status bytes are mapped to the *shared* Rust enums, which keep the FCI v10 name set
13//! ([`crate::error::MoveStatus`], [`v10::StopMoveStatus`], [`v10::AutomaticErrorRecoveryStatus`],
14//! [`v10::GetterSetterStatus`], [`v10::CommandStatus`]). Because the v5 enums are proper
15//! subsets by *name*, mapping is lossless; a byte that is not a valid status for the negotiated
16//! version is a [`crate::error::FrankaError::Protocol`].
17//!
18//! The module is split by direction: `encode` builds the `Move::Request` and the UDP
19//! `RobotCommand`, `decode` reads the UDP `RobotState`, and `status` maps response status
20//! bytes onto the shared enums. Everything is re-exported here, so `codec::*` paths are
21//! unchanged.
22
23mod decode;
24mod encode;
25mod status;
26
27pub(crate) use decode::{parse_robot_state, parse_state_modes, StateModes, ROBOT_STATE_MAX_LEN};
28pub(crate) use encode::{move_request, robot_command, RobotCommandData};
29pub(crate) use status::{
30    parse_automatic_error_recovery_status, parse_command_status, parse_getter_setter_status,
31    parse_move_status, parse_stop_move_status,
32};
33
34use crate::wire::robot::{v10, v5};
35
36/// The FCI protocol version a connection speaks.
37///
38/// `V5` is the Franka Emika Robot (FER) as served by libfranka 0.9.2
39/// (`research_interface::robot::kVersion == 5`), `V10` the Franka Research 3 as served by
40/// libfranka 0.21.2 (`kVersion == 10`).
41#[derive(Debug, Clone, Copy, PartialEq, Eq)]
42pub enum FciVersion {
43    /// Franka Emika Robot (FER), FCI version 5.
44    V5,
45    /// Franka Research 3, FCI version 10.
46    V10,
47}
48
49/// A command, named independently of the version that carries it.
50///
51/// The wire numbering is [`command_id`]; some commands exist in only one version, which is what
52/// the `Option` there expresses.
53#[derive(Debug, Clone, Copy, PartialEq, Eq)]
54pub(crate) enum CommandKind {
55    Connect,
56    Move,
57    StopMove,
58    /// FCI v5 only (`Robot::getVirtualWall`).
59    GetCartesianLimit,
60    SetCollisionBehavior,
61    SetJointImpedance,
62    SetCartesianImpedance,
63    SetGuidingMode,
64    SetEEToK,
65    SetNEToEE,
66    SetLoad,
67    /// FCI v5 only (`Robot::setFilters`).
68    SetFilters,
69    AutomaticErrorRecovery,
70    /// FCI v5 only (downloads `libfcimodels.so`).
71    LoadModelLibrary,
72    /// FCI v10 only (downloads the URDF).
73    GetRobotModel,
74}
75
76impl CommandKind {
77    /// The command name libfranka prints in its `CommandException` and `ProtocolException`
78    /// texts (`research_interface::robot::CommandTraits<T>::kName`,
79    /// `common/include/research_interface/robot/service_traits.h`).
80    ///
81    /// The two files agree on every name they share; 0.9.2 adds `Get Cartesian Limit` and
82    /// `Set Filters`, 0.21.2 adds `Get Robot Model`. `Connect` and `LoadModelLibrary` have no
83    /// `CommandTraits` specialisation in either version — neither goes through
84    /// `handleCommandResponse` — so the spellings below are this crate's own and are never
85    /// compared against a libfranka string.
86    pub(crate) const fn name(self) -> &'static str {
87        match self {
88            CommandKind::Connect => "Connect",
89            CommandKind::Move => "Move",
90            CommandKind::StopMove => "Stop Move",
91            CommandKind::GetCartesianLimit => "Get Cartesian Limit",
92            CommandKind::SetCollisionBehavior => "Set Collision Behavior",
93            CommandKind::SetJointImpedance => "Set Joint Impedance",
94            CommandKind::SetCartesianImpedance => "Set Cartesian Impedance",
95            CommandKind::SetGuidingMode => "Set Guiding Mode",
96            CommandKind::SetEEToK => "Set EE to K",
97            CommandKind::SetNEToEE => "Set NE to EE",
98            CommandKind::SetLoad => "Set Load",
99            CommandKind::SetFilters => "Set Filters",
100            CommandKind::AutomaticErrorRecovery => "Automatic Error Recovery",
101            CommandKind::LoadModelLibrary => "Load Model Library",
102            CommandKind::GetRobotModel => "Get Robot Model",
103        }
104    }
105}
106
107impl FciVersion {
108    /// The version number announced in `Connect::Request` and expected back in
109    /// `Connect::Response`.
110    pub(crate) const fn number(self) -> u16 {
111        match self {
112            FciVersion::V5 => v5::ROBOT_VERSION,
113            FciVersion::V10 => crate::wire::ROBOT_VERSION,
114        }
115    }
116}
117
118/// The `Connect::Request::version` this crate sends for `version` (5 or 10).
119pub(crate) const fn connect_version(version: FciVersion) -> u16 {
120    version.number()
121}
122
123/// Size of one UDP `RobotState` datagram: 2373 bytes on FCI v5, 1377 on FCI v10.
124pub(crate) const fn state_size(version: FciVersion) -> usize {
125    match version {
126        FciVersion::V5 => std::mem::size_of::<v5::RobotState>(),
127        FciVersion::V10 => std::mem::size_of::<v10::RobotState>(),
128    }
129}
130
131/// Size of one UDP `RobotCommand` datagram: 370 bytes on FCI v5, 371 on FCI v10.
132pub(crate) const fn command_size(version: FciVersion) -> usize {
133    match version {
134        FciVersion::V5 => std::mem::size_of::<v5::RobotCommand>(),
135        FciVersion::V10 => std::mem::size_of::<v10::RobotCommand>(),
136    }
137}
138
139/// Largest `Move::Request` of any supported version (FCI v10's 113 bytes).
140pub(crate) const MOVE_REQUEST_MAX_LEN: usize = std::mem::size_of::<v10::MoveRequest>();
141const _: () = assert!(std::mem::size_of::<v5::MoveRequest>() <= MOVE_REQUEST_MAX_LEN);
142
143/// Largest `RobotCommand` of any supported version (FCI v10's 371 bytes).
144pub(crate) const ROBOT_COMMAND_MAX_LEN: usize = std::mem::size_of::<v10::RobotCommand>();
145const _: () = assert!(std::mem::size_of::<v5::RobotCommand>() <= ROBOT_COMMAND_MAX_LEN);
146
147/// The wire `Command` value of `kind` under `version`, or `None` when the command does not
148/// exist there.
149///
150/// FCI v5 numbering is `service_types.h:20-35` of libfranka 0.9.2, FCI v10's is
151/// `service_types.h` of 0.21.2. They agree only on `Connect`, `Move` and `StopMove`.
152pub(crate) const fn command_id(version: FciVersion, kind: CommandKind) -> Option<u32> {
153    Some(match version {
154        FciVersion::V5 => match kind {
155            CommandKind::Connect => v5::Command::Connect.to_u32(),
156            CommandKind::Move => v5::Command::Move.to_u32(),
157            CommandKind::StopMove => v5::Command::StopMove.to_u32(),
158            CommandKind::GetCartesianLimit => v5::Command::GetCartesianLimit.to_u32(),
159            CommandKind::SetCollisionBehavior => v5::Command::SetCollisionBehavior.to_u32(),
160            CommandKind::SetJointImpedance => v5::Command::SetJointImpedance.to_u32(),
161            CommandKind::SetCartesianImpedance => v5::Command::SetCartesianImpedance.to_u32(),
162            CommandKind::SetGuidingMode => v5::Command::SetGuidingMode.to_u32(),
163            CommandKind::SetEEToK => v5::Command::SetEEToK.to_u32(),
164            CommandKind::SetNEToEE => v5::Command::SetNEToEE.to_u32(),
165            CommandKind::SetLoad => v5::Command::SetLoad.to_u32(),
166            CommandKind::SetFilters => v5::Command::SetFilters.to_u32(),
167            CommandKind::AutomaticErrorRecovery => v5::Command::AutomaticErrorRecovery.to_u32(),
168            CommandKind::LoadModelLibrary => v5::Command::LoadModelLibrary.to_u32(),
169            CommandKind::GetRobotModel => return None,
170        },
171        FciVersion::V10 => match kind {
172            CommandKind::Connect => v10::Command::Connect.to_u32(),
173            CommandKind::Move => v10::Command::Move.to_u32(),
174            CommandKind::StopMove => v10::Command::StopMove.to_u32(),
175            CommandKind::SetCollisionBehavior => v10::Command::SetCollisionBehavior.to_u32(),
176            CommandKind::SetJointImpedance => v10::Command::SetJointImpedance.to_u32(),
177            CommandKind::SetCartesianImpedance => v10::Command::SetCartesianImpedance.to_u32(),
178            CommandKind::SetGuidingMode => v10::Command::SetGuidingMode.to_u32(),
179            CommandKind::SetEEToK => v10::Command::SetEEToK.to_u32(),
180            CommandKind::SetNEToEE => v10::Command::SetNEToEE.to_u32(),
181            CommandKind::SetLoad => v10::Command::SetLoad.to_u32(),
182            CommandKind::AutomaticErrorRecovery => v10::Command::AutomaticErrorRecovery.to_u32(),
183            CommandKind::GetRobotModel => v10::Command::GetRobotModel.to_u32(),
184            CommandKind::GetCartesianLimit
185            | CommandKind::SetFilters
186            | CommandKind::LoadModelLibrary => return None,
187        },
188    })
189}
190
191#[cfg(test)]
192mod tests;
franka/wire/robot/codec/mod.rs

franka/wire/robot/codec/
mod.rs
