franka/robot/robot_impl/
mod.rs

1//! Connection, state stream and motion lifecycle.
2//!
3//! Port of `franka::Robot::Impl` (libfranka 0.21.2 `src/robot_impl.{h,cpp}`). Everything that
4//! talks to the robot lives here; [`crate::robot::Robot`] is a thin, locked facade on top.
5//!
6//! Unlike the C++ class, `RobotImpl` is `Sync` and every method takes `&self`: the mutable
7//! bookkeeping (message id, modes) lives behind one small mutex — the equivalent of libfranka's
8//! `message_id_mutex_`, widened to cover the mode fields it also reads and writes — so that
9//! `Robot::stop()` can run on another thread while a control loop is running.
10//!
11//! This module holds the session itself — the handshake, the shared bookkeeping and the state
12//! stream. The motion lifecycle (`Move`, the per-cycle command, `StopMove` and the
13//! `ActiveControl` write paths) is in the `motion` submodule, and libfranka's `createControlException`
14//! text formatting is in the `exception` submodule.
15
16mod exception;
17mod motion;
18
19pub(crate) use exception::create_control_exception;
20
21use std::sync::Mutex;
22
23use crate::duration::Duration;
24use crate::error::{
25    ControlException, FrankaError, FrankaResult, MoveStatus, Record, RobotCommandLog,
26};
27use crate::errors::Errors;
28use crate::joint_velocity_limits::JointVelocityLimitsConfig;
29use crate::model::Model;
30use crate::network::{connect_handshake, Network};
31use crate::realtime::{
32    has_realtime_kernel, set_current_thread_to_highest_scheduler_priority, RealtimeConfig,
33    NO_REALTIME_KERNEL_MESSAGE,
34};
35use crate::robot::commands::{
36    handle_move_response, handle_stop_move_response, handle_terminal_move_response, CommandName,
37};
38use crate::robot::logger::RobotStateLogger;
39use crate::robot::VersionPolicy;
40use crate::robot_state::{RobotMode, RobotState};
41use crate::wire::robot::codec::{self, CommandKind, FciVersion, RobotCommandData, StateModes};
42use crate::wire::robot::{
43    ControllerCommand, ControllerMode as StateControllerMode, Deviation, MotionGeneratorCommand,
44    MotionGeneratorMode as StateMotionGeneratorMode, MoveControllerMode, MoveMotionGeneratorMode,
45    RobotMode as WireRobotMode,
46};
47use crate::wire::{
48    f64s_to_f64, incorrect_object_size, message_payload, HeaderLayout, ROBOT_COMMAND_PORT,
49};
50
51/// Deviations libfranka's control loops and `ActiveControl` pass to every `Move`
52/// (`franka::ControlLoop<T>::kDefaultDeviation`).
53pub const DEFAULT_DEVIATION: (f64, f64, f64) = (10.0, 3.12, std::f64::consts::TAU);
54
55/// Number of joints, as `franka::RobotControl::kNumJoints`.
56pub const NUM_JOINTS: usize = 7;
57
58/// Mutable bookkeeping shared between the control thread and command threads.
59///
60/// libfranka guards only `message_id_` with a mutex and leaves the mode fields unsynchronised;
61/// the Rust port puts them in the same lock because the same fields are read by `stop()` on a
62/// second thread.
63#[derive(Debug)]
64struct State {
65    /// `message_id` of the newest accepted state.
66    message_id: u64,
67    /// Robot mode of the newest accepted state.
68    robot_mode: WireRobotMode,
69    /// Motion generator mode of the newest accepted state.
70    motion_generator_mode: StateMotionGeneratorMode,
71    /// Controller mode of the newest accepted state.
72    controller_mode: StateControllerMode,
73    /// Motion generator mode requested by the running `Move`.
74    current_move_motion_generator_mode: StateMotionGeneratorMode,
75    /// Controller mode requested by the running `Move`.
76    current_move_controller_mode: StateControllerMode,
77}
78
79impl State {
80    /// `Robot::Impl::motionGeneratorRunning`.
81    fn motion_generator_running(&self) -> bool {
82        self.motion_generator_mode != StateMotionGeneratorMode::Idle
83            && self.motion_generator_mode != StateMotionGeneratorMode::None
84    }
85
86    /// `Robot::Impl::controllerRunning`.
87    fn controller_running(&self) -> bool {
88        self.controller_mode == StateControllerMode::ExternalController
89    }
90
91    /// The `Move` we started is fully active, i.e. the state reports both requested modes.
92    fn move_active(&self) -> bool {
93        self.motion_generator_mode == self.current_move_motion_generator_mode
94            && self.controller_mode == self.current_move_controller_mode
95    }
96}
97
98/// The FCI session plus everything `franka::Robot::Impl` keeps.
99#[derive(Debug)]
100pub struct RobotImpl {
101    network: Network,
102    logger: Mutex<RobotStateLogger>,
103    realtime_config: RealtimeConfig,
104    /// FCI protocol version negotiated at connect time. Every wire touch below goes through
105    /// [`crate::wire::robot::codec`] with this value.
106    version: FciVersion,
107    ri_version: u16,
108    state: Mutex<State>,
109    /// Position-dependent joint velocity envelope from the URDF. FCI v5 has no `GetRobotModel`
110    /// and no such envelope, so on an FER this stays at its default and
111    /// [`RobotImpl::upper_joint_velocity_limits`] answers from `rate_limiting::fer` instead.
112    joint_velocity_limits: JointVelocityLimitsConfig,
113    /// The URDF `GetRobotModel` returned; empty on FCI v5, which has no such command.
114    robot_model_urdf: String,
115}
116
117impl RobotImpl {
118    /// Connects to `franka_address` and performs libfranka's full startup sequence with the
119    /// default [`VersionPolicy::Auto`].
120    pub fn new(
121        franka_address: &str,
122        realtime_config: RealtimeConfig,
123        log_size: usize,
124    ) -> FrankaResult<RobotImpl> {
125        RobotImpl::new_with_policy(
126            franka_address,
127            realtime_config,
128            log_size,
129            VersionPolicy::default(),
130        )
131    }
132
133    /// [`RobotImpl::new`] with an explicit [`VersionPolicy`].
134    ///
135    /// Port of `Robot::Impl::Impl` for both supported protocol versions: raise the calling
136    /// thread to the highest realtime priority (fatal only with [`RealtimeConfig::Enforce`]),
137    /// check the kernel, run the `Connect` handshake and wait for the first robot state.
138    ///
139    /// On FCI v10 the URDF is then fetched with `GetRobotModel` and the position-dependent
140    /// joint velocity limits are derived from it, exactly as libfranka 0.21.2 does. FCI v5 has
141    /// neither command nor envelope — libfranka 0.9.2's constructor stops after the first state
142    /// (`src/robot_impl.cpp:19-27`) — so the FER path skips both.
143    ///
144    /// [`VersionPolicy::Auto`] tries FCI v10 first and, if the server answers
145    /// `kIncompatibleLibraryVersion` reporting version 5, closes both sockets and reconnects
146    /// once as FCI v5. Any other server version is returned as the
147    /// [`FrankaError::IncompatibleVersion`] it is.
148    pub fn new_with_policy(
149        franka_address: &str,
150        realtime_config: RealtimeConfig,
151        log_size: usize,
152        policy: VersionPolicy,
153    ) -> FrankaResult<RobotImpl> {
154        let throw_on_error = realtime_config == RealtimeConfig::Enforce;
155        if let Err(message) = set_current_thread_to_highest_scheduler_priority() {
156            if throw_on_error {
157                log_error(&message);
158                return Err(FrankaError::Realtime(message));
159            }
160        }
161        if throw_on_error && !has_realtime_kernel() {
162            log_error(NO_REALTIME_KERNEL_MESSAGE);
163            return Err(FrankaError::Realtime(
164                NO_REALTIME_KERNEL_MESSAGE.to_string(),
165            ));
166        }
167
168        let (network, ri_version, version) = match policy {
169            VersionPolicy::Exact(version) => {
170                let (network, ri_version) = connect_as(franka_address, version)?;
171                (network, ri_version, version)
172            }
173            VersionPolicy::Auto => match connect_as(franka_address, FciVersion::V10) {
174                Ok((network, ri_version)) => (network, ri_version, FciVersion::V10),
175                // `connect_as` owns the failed session and has already dropped it, so both
176                // sockets are closed before the second connection is opened.
177                Err(FrankaError::IncompatibleVersion {
178                    server_version: 5, ..
179                }) => {
180                    let (network, ri_version) = connect_as(franka_address, FciVersion::V5)?;
181                    (network, ri_version, FciVersion::V5)
182                }
183                Err(other) => return Err(other),
184            },
185        };
186
187        let robot = RobotImpl {
188            network,
189            logger: Mutex::new(RobotStateLogger::new(log_size)),
190            realtime_config,
191            version,
192            ri_version,
193            state: Mutex::new(State {
194                message_id: 0,
195                robot_mode: WireRobotMode::Other,
196                motion_generator_mode: StateMotionGeneratorMode::Idle,
197                controller_mode: StateControllerMode::Other,
198                current_move_motion_generator_mode: StateMotionGeneratorMode::Idle,
199                current_move_controller_mode: StateControllerMode::Other,
200            }),
201            joint_velocity_limits: JointVelocityLimitsConfig::default(),
202            robot_model_urdf: String::new(),
203        };
204
205        // `updateState(network_->udpBlockingReceive<RobotState>())`
206        let mut buffer = [0u8; codec::ROBOT_STATE_MAX_LEN];
207        let size = codec::state_size(version);
208        let received = robot.network.blocking_receive_bytes(&mut buffer)?;
209        if received != size {
210            return Err(incorrect_object_size());
211        }
212        robot.update_state(&codec::parse_state_modes(version, &buffer[..size])?);
213
214        if version == FciVersion::V5 {
215            return Ok(robot);
216        }
217
218        let urdf = robot.get_robot_model()?;
219        let joint_velocity_limits = JointVelocityLimitsConfig::from_urdf(&urdf)?;
220
221        Ok(RobotImpl {
222            robot_model_urdf: urdf,
223            joint_velocity_limits,
224            ..robot
225        })
226    }
227
228    /// The FCI protocol version this session speaks (`Robot::fci_version`).
229    pub fn version(&self) -> FciVersion {
230        self.version
231    }
232
233    /// The FCI version reported by the server (`Robot::Impl::serverVersion`).
234    pub fn server_version(&self) -> u16 {
235        self.ri_version
236    }
237
238    /// The realtime configuration this instance was created with
239    /// (`Robot::Impl::realtimeConfig`).
240    pub fn realtime_config(&self) -> RealtimeConfig {
241        self.realtime_config
242    }
243
244    /// The URDF fetched at connection time (`Robot::Impl::robotModelUrdf`).
245    pub fn robot_model_urdf(&self) -> &str {
246        &self.robot_model_urdf
247    }
248
249    /// The TCP/UDP session, for the command implementations in
250    /// [`crate::robot::commands`].
251    pub(crate) fn network(&self) -> &Network {
252        &self.network
253    }
254
255    /// Upper joint velocity limits at `q` (`Robot::Impl::getUpperJointVelocityLimits`).
256    ///
257    /// FCI v5 has no position-dependent envelope: libfranka 0.9.2 rate limits against the flat
258    /// `kMaxJointVelocity` of `include/franka/rate_limiting.h`, so `q` is ignored there.
259    pub fn upper_joint_velocity_limits(&self, q: &[f64; NUM_JOINTS]) -> [f64; NUM_JOINTS] {
260        match self.version {
261            FciVersion::V5 => crate::rate_limiting::fer::MAX_JOINT_VELOCITY,
262            FciVersion::V10 => self.joint_velocity_limits.upper_limits(q),
263        }
264    }
265
266    /// Lower joint velocity limits at `q` (`Robot::Impl::getLowerJointVelocityLimits`).
267    ///
268    /// See [`RobotImpl::upper_joint_velocity_limits`] for the FCI v5 case.
269    pub fn lower_joint_velocity_limits(&self, q: &[f64; NUM_JOINTS]) -> [f64; NUM_JOINTS] {
270        match self.version {
271            FciVersion::V5 => crate::rate_limiting::fer::MIN_JOINT_VELOCITY,
272            FciVersion::V10 => self.joint_velocity_limits.lower_limits(q),
273        }
274    }
275
276    /// Loads the FER's model library over `LoadModelLibrary` (FCI v5's `Robot::loadModel`).
277    ///
278    /// Downloads `libfcimodels.so` from the robot and binds its thirty symbols
279    /// ([`crate::model::load_from_robot`]).
280    pub fn load_model_v5(&self) -> FrankaResult<Model> {
281        crate::model::load_from_robot(&self.network, self.version)
282    }
283
284    /// The wire command id of `name` under the negotiated version.
285    ///
286    /// # Errors
287    /// [`FrankaError::InvalidOperation`] when the command does not exist in this FCI version —
288    /// `Get Robot Model` on an FER, `Set Filters` and `Get Cartesian Limit` on an FR3.
289    pub(crate) fn command_id(&self, name: CommandName) -> FrankaResult<u32> {
290        codec::command_id(self.version, name.kind()).ok_or_else(|| {
291            FrankaError::InvalidOperation(format!(
292                "libfranka: {} is not available on FCI version {}.",
293                name.as_str(),
294                self.version.number()
295            ))
296        })
297    }
298
299    /// The current robot mode, used by `commandNotPossibleMsg`.
300    pub(crate) fn robot_mode(&self) -> RobotMode {
301        let mode = self.lock().robot_mode;
302        RobotMode::from_u8(mode.to_u8()).unwrap_or(RobotMode::Other)
303    }
304
305    fn lock(&self) -> std::sync::MutexGuard<'_, State> {
306        self.state.lock().unwrap_or_else(|e| e.into_inner())
307    }
308
309    fn logger(&self) -> std::sync::MutexGuard<'_, RobotStateLogger> {
310        self.logger.lock().unwrap_or_else(|e| e.into_inner())
311    }
312
313    /// `Robot::Impl::updateState`.
314    fn update_state(&self, modes: &StateModes) {
315        let mut guard = self.lock();
316        guard.robot_mode = WireRobotMode::from_u8(modes.robot_mode).unwrap_or(WireRobotMode::Other);
317        guard.motion_generator_mode =
318            StateMotionGeneratorMode::from_u8(modes.motion_generator_mode)
319                .unwrap_or(StateMotionGeneratorMode::Idle);
320        guard.controller_mode = StateControllerMode::from_u8(modes.controller_mode)
321            .unwrap_or(StateControllerMode::Other);
322        guard.message_id = modes.message_id;
323    }
324
325    /// Receives the newest robot state (`Robot::Impl::receiveRobotState`).
326    ///
327    /// Drains everything already queued on the socket keeping the highest `message_id`, then
328    /// blocks until a state newer than the last accepted one arrives.
329    ///
330    /// Both buffers are sized for the largest supported `RobotState` and every datagram's
331    /// length is compared against the negotiated version's, so a state of the *other* version
332    /// fails with libfranka's `Protocol("libfranka: incorrect object size")`
333    /// (`src/network.h:140-142`) instead of being truncated into a plausible-looking state.
334    /// Nothing here allocates: the two buffers live on the caller's stack.
335    fn receive_robot_state(&self) -> FrankaResult<RobotState> {
336        let size = codec::state_size(self.version);
337        let last_message_id = self.lock().message_id;
338        let mut buffer = [0u8; codec::ROBOT_STATE_MAX_LEN];
339        let mut latest = [0u8; codec::ROBOT_STATE_MAX_LEN];
340        let mut latest_message_id = last_message_id;
341
342        while let Some(received) = self.network.try_receive_bytes(&mut buffer)? {
343            if received != size {
344                return Err(incorrect_object_size());
345            }
346            let modes = codec::parse_state_modes(self.version, &buffer[..size])?;
347            if modes.message_id > latest_message_id {
348                latest_message_id = modes.message_id;
349                latest[..size].copy_from_slice(&buffer[..size]);
350            }
351        }
352
353        while latest_message_id == last_message_id {
354            let received = self.network.blocking_receive_bytes(&mut buffer)?;
355            if received != size {
356                return Err(incorrect_object_size());
357            }
358            let modes = codec::parse_state_modes(self.version, &buffer[..size])?;
359            if modes.message_id > latest_message_id {
360                latest_message_id = modes.message_id;
361                latest[..size].copy_from_slice(&buffer[..size]);
362            }
363        }
364
365        self.update_state(&codec::parse_state_modes(self.version, &latest[..size])?);
366        codec::parse_robot_state(self.version, &latest[..size])
367    }
368}
369
370/// Opens a fresh FCI session and runs the `Connect` handshake for `version`.
371///
372/// The [`Network`] is owned by this function, so a failing handshake — an
373/// [`FrankaError::IncompatibleVersion`] in particular — drops both sockets before the error
374/// reaches the caller. That is what makes [`VersionPolicy::Auto`]'s retry a *new* connection
375/// rather than a second handshake on the socket the server just rejected.
376///
377/// **Deviation from libfranka**, which only inspects the `Connect` *status*: a handshake that
378/// succeeded but whose response reports a different protocol version is turned into the same
379/// [`FrankaError::IncompatibleVersion`] as an outright rejection. Everything after the
380/// handshake is decoded against `version`, so a server that answers `kSuccess` while speaking
381/// another version would otherwise be met with 2373-byte states on a 1377-byte session — which
382/// surfaces as `Protocol("libfranka: incorrect object size")` on the very first state instead
383/// of naming the real problem. franka-sim's FER build does exactly this (it answers
384/// `kSuccess` with version 5 whatever the client announced), and it is what lets
385/// [`VersionPolicy::Auto`] recognise an FER there.
386fn connect_as(franka_address: &str, version: FciVersion) -> FrankaResult<(Network, u16)> {
387    // `connect_handshake` sends the shared `Connect` id rather than asking the codec for it,
388    // which is only correct because both versions number `Connect` as 0.
389    const _: () = assert!(matches!(
390        codec::command_id(FciVersion::V5, CommandKind::Connect),
391        Some(0)
392    ));
393    const _: () = assert!(matches!(
394        codec::command_id(FciVersion::V10, CommandKind::Connect),
395        Some(0)
396    ));
397
398    let library_version = codec::connect_version(version);
399    let network = Network::connect(franka_address, ROBOT_COMMAND_PORT, HeaderLayout::Robot)?;
400    let ri_version = connect_handshake(&network, library_version)?;
401    if ri_version != library_version {
402        return Err(FrankaError::IncompatibleVersion {
403            server_version: ri_version,
404            library_version,
405        });
406    }
407    Ok((network, ri_version))
408}
409
410/// The version-agnostic log record of one sent command.
411fn command_log(command: &RobotCommandData) -> RobotCommandLog {
412    RobotCommandLog {
413        q_c: command.q_c,
414        dq_c: command.dq_c,
415        O_T_EE_c: command.O_T_EE_c,
416        O_dP_EE_c: command.O_dP_EE_c,
417        elbow_c: command.elbow_c,
418        tau_J_d: command.tau_J_d,
419    }
420}
421
422/// Builds a [`FrankaError::Control`] without a log, as the C++ `ControlException(msg)` does.
423pub(crate) fn control_error(message: &str) -> FrankaError {
424    FrankaError::Control(ControlException::new(message))
425}
426
427/// First payload byte of a command response.
428pub(crate) fn status_byte(message: &[u8]) -> FrankaResult<u8> {
429    let payload = message_payload(HeaderLayout::Robot, message);
430    payload
431        .first()
432        .copied()
433        .ok_or_else(|| FrankaError::Protocol("libfranka: Incorrect TCP message size.".to_string()))
434}
435
436/// libfranka's `logging::logError`, which writes to `std::cerr` through the default sink.
437pub(crate) fn log_error(message: &str) {
438    eprintln!("{message}");
439}
440
441/// libfranka's `logging::logWarn`.
442pub(crate) fn log_warn(message: &str) {
443    eprintln!("{message}");
444}
445
446/// The `franka::Duration` between two states, used by `ActiveControl::readOnce`.
447pub(crate) fn time_since(previous: Option<Duration>, now: Duration) -> Duration {
448    match previous {
449        Some(previous) => now - previous,
450        None => Duration::default(),
451    }
452}
453
454#[cfg(test)]
455mod tests;
franka/robot/robot_impl/mod.rs

franka/robot/robot_impl/
mod.rs
