franka/
error.rs

1//! Error types (mirror libfranka's exception hierarchy).
2
3use crate::errors::Errors;
4use crate::robot_state::RobotState;
5
6/// Result alias used throughout the crate.
7pub type FrankaResult<T> = Result<T, FrankaError>;
8
9/// Terminal status of a `Move` command (`research_interface::robot::Move::Status`).
10#[derive(Debug, Clone, Copy, PartialEq, Eq)]
11#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
12pub enum MoveStatus {
13    /// The motion ended regularly; this is the only reply that makes a control loop return
14    /// `Ok(())` (`kSuccess`).
15    Success,
16    /// The robot accepted the `Move` and the motion is now running (`kMotionStarted`). Sent
17    /// once, as the first reply.
18    MotionStarted,
19    /// The motion was pre-empted by another command, typically `StopMove` (`kPreempted`).
20    Preempted,
21    /// The motion was pre-empted because a safety function became active
22    /// (`kPreemptedDueToActivatedSafetyFunctions`, FCI v10 only).
23    PreemptedDueToActivatedSafetyFunctions,
24    /// The `Move` was refused because a safety function is active
25    /// (`kCommandRejectedDueToActivatedSafetyFunctions`, FCI v10 only).
26    CommandRejectedDueToActivatedSafetyFunctions,
27    /// The robot is not in a mode that allows a motion, e.g. it is still in a reflex or guiding
28    /// mode (`kCommandNotPossibleRejected`).
29    CommandNotPossibleRejected,
30    /// The `Move` was refused because the robot starts at a singular pose
31    /// (`kStartAtSingularPoseRejected`).
32    StartAtSingularPoseRejected,
33    /// The `Move` arguments were out of range (`kInvalidArgumentRejected`).
34    InvalidArgumentRejected,
35    /// A reflex (collision, joint or Cartesian limit violation) aborted the motion
36    /// (`kReflexAborted`); [`ControlException::last_motion_errors`] names which.
37    ReflexAborted,
38    /// The external emergency stop or the user stop aborted the motion (`kEmergencyAborted`).
39    EmergencyAborted,
40    /// The robot rejected a command the client sent during the motion (`kInputErrorAborted`),
41    /// e.g. a setpoint that violates the rate limits.
42    InputErrorAborted,
43    /// The motion was aborted for a reason none of the above covers (`kAborted`).
44    Aborted,
45}
46
47impl MoveStatus {
48    /// Wire value (`u8`).
49    pub const fn to_u8(self) -> u8 {
50        self as u8
51    }
52
53    /// Parses the wire value.
54    pub const fn from_u8(v: u8) -> Option<MoveStatus> {
55        Some(match v {
56            0 => MoveStatus::Success,
57            1 => MoveStatus::MotionStarted,
58            2 => MoveStatus::Preempted,
59            3 => MoveStatus::PreemptedDueToActivatedSafetyFunctions,
60            4 => MoveStatus::CommandRejectedDueToActivatedSafetyFunctions,
61            5 => MoveStatus::CommandNotPossibleRejected,
62            6 => MoveStatus::StartAtSingularPoseRejected,
63            7 => MoveStatus::InvalidArgumentRejected,
64            8 => MoveStatus::ReflexAborted,
65            9 => MoveStatus::EmergencyAborted,
66            10 => MoveStatus::InputErrorAborted,
67            11 => MoveStatus::Aborted,
68            _ => return None,
69        })
70    }
71}
72
73/// The robot command that was sent in one cycle, as recorded in the control log.
74#[derive(Debug, Clone, Copy, Default, PartialEq)]
75#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
76pub struct RobotCommandLog {
77    /// Commanded joint positions, in rad.
78    pub q_c: [f64; 7],
79    /// Commanded joint velocities, in rad/s.
80    pub dq_c: [f64; 7],
81    /// Commanded end-effector pose in base frame, a column-major 4x4 homogeneous transform
82    /// (translation in m).
83    pub O_T_EE_c: [f64; 16],
84    /// Commanded end-effector twist in base frame: linear m/s then angular rad/s.
85    pub O_dP_EE_c: [f64; 6],
86    /// Commanded elbow configuration: joint-3 position in rad and the sign of joint 4.
87    pub elbow_c: [f64; 2],
88    /// Commanded joint torques without gravity and friction, in Nm.
89    pub tau_J_d: [f64; 7],
90}
91
92/// One entry of the control log attached to a [`ControlException`] (mirrors `franka::Record`).
93///
94/// With the `serde` feature a `Vec<Record>` is what a control log looks like on disk.
95#[derive(Debug, Clone, PartialEq)]
96#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
97pub struct Record {
98    /// The robot state received in this cycle.
99    pub state: RobotState,
100    /// The command sent in this cycle, or `None` when the cycle sent nothing.
101    pub command: Option<RobotCommandLog>,
102}
103
104/// A motion ended abnormally (mirrors `franka::ControlException`).
105///
106/// With the `serde` feature the whole exception serialises, log included; nothing in it is
107/// unserialisable.
108#[derive(Debug, Clone)]
109#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
110pub struct ControlException {
111    /// Full libfranka-style message, including the error names and success-rate lines when
112    /// the motion was aborted by a reflex.
113    pub message: String,
114    /// Terminal `Move` status when the exception was produced by a `Move` response.
115    pub move_status: Option<MoveStatus>,
116    /// `reflex_reason` of the last received state.
117    pub last_motion_errors: Errors,
118    /// The last states and commands before the exception (newest last).
119    pub log: Vec<Record>,
120}
121
122impl ControlException {
123    /// Convenience constructor for an exception without a log.
124    pub fn new(message: impl Into<String>) -> Self {
125        ControlException {
126            message: message.into(),
127            move_status: None,
128            last_motion_errors: Errors::default(),
129            log: Vec::new(),
130        }
131    }
132}
133
134impl std::fmt::Display for ControlException {
135    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
136        f.write_str(&self.message)
137    }
138}
139
140impl std::error::Error for ControlException {}
141
142/// Every error the crate can return (mirrors libfranka's exception types).
143#[derive(Debug, Clone, thiserror::Error)]
144pub enum FrankaError {
145    /// Connection or socket failure (`franka::NetworkException`).
146    #[error("{0}")]
147    Network(String),
148    /// Malformed or unexpected protocol data (`franka::ProtocolException`).
149    #[error("{0}")]
150    Protocol(String),
151    /// The server speaks another FCI version (`franka::IncompatibleVersionException`).
152    #[error("libfranka: Incompatible library version (server version: {server_version}, library version: {library_version}).")]
153    IncompatibleVersion {
154        /// FCI version the control box announced in its `Connect` reply.
155        server_version: u16,
156        /// FCI version this client requested (5 or 10).
157        library_version: u16,
158    },
159    /// A TCP command was rejected (`franka::CommandException`).
160    #[error("{0}")]
161    Command(String),
162    /// A motion ended abnormally (`franka::ControlException`).
163    #[error("{0}")]
164    Control(ControlException),
165    /// Realtime priority or kernel requirements not met (`franka::RealtimeException`).
166    #[error("{0}")]
167    Realtime(String),
168    /// Operation not allowed in the current state (`franka::InvalidOperationException`).
169    #[error("{0}")]
170    InvalidOperation(String),
171    /// Invalid user input (`std::invalid_argument` in libfranka).
172    #[error("{0}")]
173    InvalidArgument(String),
174    /// Model loading or evaluation failure (`franka::ModelException`).
175    #[error("{0}")]
176    Model(String),
177}
178
179impl From<ControlException> for FrankaError {
180    fn from(e: ControlException) -> Self {
181        FrankaError::Control(e)
182    }
183}
184
185#[cfg(test)]
186mod tests {
187    use super::*;
188
189    #[test]
190    fn incompatible_version_text_matches_libfranka() {
191        let e = FrankaError::IncompatibleVersion {
192            server_version: 10,
193            library_version: 9,
194        };
195        assert_eq!(
196            e.to_string(),
197            "libfranka: Incompatible library version (server version: 10, library version: 9)."
198        );
199    }
200
201    #[test]
202    fn move_status_roundtrip() {
203        for v in 0..=11u8 {
204            let s = MoveStatus::from_u8(v).unwrap();
205            assert_eq!(s.to_u8(), v);
206        }
207        assert!(MoveStatus::from_u8(12).is_none());
208    }
209
210    #[cfg(feature = "serde")]
211    #[test]
212    fn record_and_exception_round_trip_through_json() {
213        use crate::duration::Duration;
214        use crate::robot_state::RobotMode;
215
216        let mut state = RobotState {
217            time: Duration::from_millis(123_456),
218            robot_mode: RobotMode::Reflex,
219            ..RobotState::default()
220        };
221        state.q = [0.1, -0.2, 0.3, -2.0, 0.5, 1.6, 0.7];
222        state.joint_contact[3] = 1.0;
223        state.O_F_ext_hat_K = [1.0, 2.0, 3.0, 0.1, 0.2, 0.3];
224        state.current_errors.0[Errors::index_of("cartesian_reflex").unwrap()] = true;
225        let record = Record {
226            state,
227            command: Some(RobotCommandLog {
228                q_c: [0.1, -0.2, 0.3, -2.0, 0.5, 1.6, 0.7],
229                ..RobotCommandLog::default()
230            }),
231        };
232        let json = serde_json::to_string(&record).unwrap();
233        assert!(
234            json.contains("\"current_errors\":[\"cartesian_reflex\"]"),
235            "{json}"
236        );
237        assert!(json.contains("\"time\":123456"), "{json}");
238        let back: Record = serde_json::from_str(&json).unwrap();
239        assert_eq!(back, record);
240        assert_eq!(back.state.q[3], -2.0);
241        assert_eq!(back.state.joint_contact[3], 1.0);
242        assert_eq!(back.state.current_errors, state.current_errors);
243        assert_eq!(back.command.unwrap().q_c[5], 1.6);
244
245        let exception = ControlException {
246            message: "libfranka: Move command aborted: motion aborted by reflex!".into(),
247            move_status: Some(MoveStatus::ReflexAborted),
248            last_motion_errors: state.current_errors,
249            log: vec![record.clone(), record],
250        };
251        let json = serde_json::to_string(&exception).unwrap();
252        let back: ControlException = serde_json::from_str(&json).unwrap();
253        assert_eq!(back.message, exception.message);
254        assert_eq!(back.move_status, Some(MoveStatus::ReflexAborted));
255        assert_eq!(back.last_motion_errors, exception.last_motion_errors);
256        assert_eq!(back.log, exception.log);
257    }
258}
franka/error.rs

franka/
error.rs
