franka/
errors.rs

1//! The 41 robot error flags, in the order of libfranka's `research_interface::robot::Error`.
2
3/// Number of error flags carried in every `RobotState`.
4pub const ERROR_COUNT: usize = 41;
5
6/// Error names in wire order (index = bit position in `RobotState.errors` / `reflex_reason`).
7///
8/// Source: `common/include/research_interface/robot/error.h` (enum order) and its
9/// `getErrorName` table, libfranka 0.21.2.
10pub const ERROR_NAMES: [&str; ERROR_COUNT] = [
11    "joint_position_limits_violation",
12    "cartesian_position_limits_violation",
13    "self_collision_avoidance_violation",
14    "joint_velocity_violation",
15    "cartesian_velocity_violation",
16    "force_control_safety_violation",
17    "joint_reflex",
18    "cartesian_reflex",
19    "max_goal_pose_deviation_violation",
20    "max_path_pose_deviation_violation",
21    "cartesian_velocity_profile_safety_violation",
22    "joint_position_motion_generator_start_pose_invalid",
23    "joint_motion_generator_position_limits_violation",
24    "joint_motion_generator_velocity_limits_violation",
25    "joint_motion_generator_velocity_discontinuity",
26    "joint_motion_generator_acceleration_discontinuity",
27    "cartesian_position_motion_generator_start_pose_invalid",
28    "cartesian_motion_generator_elbow_limit_violation",
29    "cartesian_motion_generator_velocity_limits_violation",
30    "cartesian_motion_generator_velocity_discontinuity",
31    "cartesian_motion_generator_acceleration_discontinuity",
32    "cartesian_motion_generator_elbow_sign_inconsistent",
33    "cartesian_motion_generator_start_elbow_invalid",
34    "force_controller_desired_force_tolerance_violation",
35    "start_elbow_sign_inconsistent",
36    "communication_constraints_violation",
37    "power_limit_violation",
38    "cartesian_motion_generator_joint_position_limits_violation",
39    "cartesian_motion_generator_joint_velocity_limits_violation",
40    "cartesian_motion_generator_joint_velocity_discontinuity",
41    "cartesian_motion_generator_joint_acceleration_discontinuity",
42    "cartesian_position_motion_generator_invalid_frame_flag",
43    "controller_torque_discontinuity",
44    "joint_p2p_insufficient_torque_for_planning",
45    "tau_J_range_violation",
46    "instability_detected",
47    "joint_move_in_wrong_direction",
48    "cartesian_spline_motion_generator_violation",
49    "joint_via_motion_generator_planning_joint_limit_violation",
50    "base_acceleration_initialization_timeout",
51    "base_acceleration_invalid_reading",
52];
53
54/// Set of robot error flags (mirrors `franka::Errors`).
55///
56/// With the `serde` feature it serialises as the list of the set flags' names in wire order
57/// (`["joint_reflex", "cartesian_reflex"]`, `[]` when empty) rather than as 41 booleans, and
58/// deserialising a name that is not in [`ERROR_NAMES`] is an error.
59#[derive(Debug, Clone, Copy, PartialEq, Eq)]
60pub struct Errors(pub [bool; ERROR_COUNT]);
61
62impl Default for Errors {
63    fn default() -> Self {
64        Errors([false; ERROR_COUNT])
65    }
66}
67
68impl Errors {
69    /// True if any flag is set (mirrors `franka::Errors::operator bool`).
70    pub fn any(&self) -> bool {
71        self.0.iter().any(|&e| e)
72    }
73
74    /// Names of the set flags, in wire order.
75    pub fn names(&self) -> Vec<&'static str> {
76        self.0
77            .iter()
78            .zip(ERROR_NAMES.iter())
79            .filter_map(|(&set, &name)| if set { Some(name) } else { None })
80            .collect()
81    }
82
83    /// Whether the flag with the given libfranka name is set. Unknown names are `false`.
84    pub fn get(&self, name: &str) -> bool {
85        index_of(name).map(|i| self.0[i]).unwrap_or(false)
86    }
87
88    /// Index of the flag with the given name.
89    pub fn index_of(name: &str) -> Option<usize> {
90        index_of(name)
91    }
92}
93
94fn index_of(name: &str) -> Option<usize> {
95    ERROR_NAMES.iter().position(|&n| n == name)
96}
97
98#[cfg(feature = "serde")]
99impl serde::Serialize for Errors {
100    fn serialize<S: serde::Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
101        serializer.collect_seq(self.names())
102    }
103}
104
105#[cfg(feature = "serde")]
106impl<'de> serde::Deserialize<'de> for Errors {
107    fn deserialize<D: serde::Deserializer<'de>>(deserializer: D) -> Result<Errors, D::Error> {
108        let names: Vec<std::borrow::Cow<'de, str>> = serde::Deserialize::deserialize(deserializer)?;
109        let mut flags = [false; ERROR_COUNT];
110        for name in &names {
111            let index = index_of(name).ok_or_else(|| {
112                serde::de::Error::custom(format!("{name:?} is not a libfranka error name"))
113            })?;
114            flags[index] = true;
115        }
116        Ok(Errors(flags))
117    }
118}
119
120impl From<[bool; ERROR_COUNT]> for Errors {
121    fn from(flags: [bool; ERROR_COUNT]) -> Self {
122        Errors(flags)
123    }
124}
125
126impl From<[u8; ERROR_COUNT]> for Errors {
127    fn from(flags: [u8; ERROR_COUNT]) -> Self {
128        let mut out = [false; ERROR_COUNT];
129        for (o, &f) in out.iter_mut().zip(flags.iter()) {
130            *o = f != 0;
131        }
132        Errors(out)
133    }
134}
135
136/// Formats like libfranka's `Errors::operator std::string`: `["name_a", "name_b"]`, `[]` when empty.
137impl std::fmt::Display for Errors {
138    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
139        write!(f, "[")?;
140        let mut first = true;
141        for name in self.names() {
142            if !first {
143                write!(f, ", ")?;
144            }
145            first = false;
146            write!(f, "\"{name}\"")?;
147        }
148        write!(f, "]")
149    }
150}
151
152#[cfg(test)]
153mod tests {
154    use super::*;
155
156    #[test]
157    fn has_41_unique_names() {
158        let mut sorted = ERROR_NAMES.to_vec();
159        sorted.sort_unstable();
160        sorted.dedup();
161        assert_eq!(sorted.len(), ERROR_COUNT);
162    }
163
164    #[test]
165    fn indices_match_libfranka_and_sim() {
166        assert_eq!(Errors::index_of("joint_velocity_violation"), Some(3));
167        assert_eq!(
168            Errors::index_of("communication_constraints_violation"),
169            Some(25)
170        );
171        assert_eq!(
172            Errors::index_of("controller_torque_discontinuity"),
173            Some(32)
174        );
175        assert_eq!(Errors::index_of("tau_J_range_violation"), Some(34));
176        assert_eq!(
177            Errors::index_of("base_acceleration_invalid_reading"),
178            Some(40)
179        );
180    }
181
182    #[test]
183    fn display_matches_libfranka_format() {
184        let mut flags = [false; ERROR_COUNT];
185        assert_eq!(Errors(flags).to_string(), "[]");
186        flags[3] = true;
187        flags[25] = true;
188        let errors = Errors(flags);
189        assert_eq!(
190            errors.to_string(),
191            "[\"joint_velocity_violation\", \"communication_constraints_violation\"]"
192        );
193        assert!(errors.any());
194        assert!(errors.get("joint_velocity_violation"));
195        assert!(!errors.get("joint_reflex"));
196        assert!(!errors.get("no_such_error"));
197    }
198
199    #[cfg(feature = "serde")]
200    #[test]
201    fn serialises_as_the_set_names() {
202        let mut flags = [false; ERROR_COUNT];
203        flags[6] = true;
204        flags[40] = true;
205        let errors = Errors(flags);
206        let json = serde_json::to_string(&errors).unwrap();
207        assert_eq!(
208            json,
209            "[\"joint_reflex\",\"base_acceleration_invalid_reading\"]"
210        );
211        assert_eq!(serde_json::from_str::<Errors>(&json).unwrap(), errors);
212        assert_eq!(
213            serde_json::from_str::<Errors>("[]").unwrap(),
214            Errors::default()
215        );
216        let error = serde_json::from_str::<Errors>("[\"no_such_error\"]").unwrap_err();
217        assert!(error.to_string().contains("no_such_error"), "{error}");
218    }
219}
franka/errors.rs

franka/
errors.rs
