franka/robot_state/
mod.rs

1//! Public robot state (mirrors `franka::RobotState`, all values `f64`).
2//!
3//! The conversion from the packed wire struct (`RobotState::from_wire`) lives in this file too
4//! and is added together with the wire layer.
5
6use crate::duration::Duration;
7use crate::errors::Errors;
8
9/// Robot mode (`franka::RobotMode`).
10#[derive(Debug, Clone, Copy, PartialEq, Eq)]
11#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
12pub enum RobotMode {
13    /// A mode this client does not model (`kOther`).
14    Other,
15    /// Ready, with no motion and no external controller running (`kIdle`).
16    Idle,
17    /// A motion generator and/or an external controller is running (`kMove`).
18    Move,
19    /// Hand-guiding mode: the brakes are open and the robot follows external forces
20    /// (`kGuiding`).
21    Guiding,
22    /// A reflex (collision or limit violation) fired and the robot stopped (`kReflex`); clear it
23    /// with [`crate::Robot::automatic_error_recovery`].
24    Reflex,
25    /// The user stop (the external enabling device) is pressed (`kUserStopped`).
26    UserStopped,
27    /// An [`crate::Robot::automatic_error_recovery`] is in progress
28    /// (`kAutomaticErrorRecovery`).
29    AutomaticErrorRecovery,
30}
31
32impl RobotMode {
33    /// Parses the wire value (`research_interface::robot::RobotMode`).
34    pub const fn from_u8(v: u8) -> Option<RobotMode> {
35        Some(match v {
36            0 => RobotMode::Other,
37            1 => RobotMode::Idle,
38            2 => RobotMode::Move,
39            3 => RobotMode::Guiding,
40            4 => RobotMode::Reflex,
41            5 => RobotMode::UserStopped,
42            6 => RobotMode::AutomaticErrorRecovery,
43            _ => return None,
44        })
45    }
46}
47
48/// libfranka prints robot modes in the `commandNotPossibleMsg` text; keep the same spelling.
49impl std::fmt::Display for RobotMode {
50    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
51        let s = match self {
52            RobotMode::Other => "Other",
53            RobotMode::Idle => "Idle",
54            RobotMode::Move => "Move",
55            RobotMode::Guiding => "Guiding",
56            RobotMode::Reflex => "Reflex",
57            RobotMode::UserStopped => "User stopped",
58            RobotMode::AutomaticErrorRecovery => "Automatic error recovery",
59        };
60        f.write_str(s)
61    }
62}
63
64/// Identity 4x4 transform in column-major order.
65pub const IDENTITY_TRANSFORM: [f64; 16] = [
66    1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
67];
68
69/// Describes the robot state (field names and semantics identical to `franka::RobotState`;
70/// see libfranka's `robot_state.h` for the documentation of every field).
71///
72/// With the `serde` feature the state serialises field by field under these names, the
73/// arrays as arrays, `time` as milliseconds and the two [`Errors`] as lists of names.
74#[derive(Debug, Clone, Copy, PartialEq)]
75#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
76pub struct RobotState {
77    /// Measured end-effector pose in base frame, column-major 4x4 homogeneous transform (translation in m).
78    pub O_T_EE: [f64; 16],
79    /// Last *desired* end-effector pose in base frame, column-major 4x4 (m). Not for motion control -- see libfranka's warning.
80    pub O_T_EE_d: [f64; 16],
81    /// Flange-to-end-effector transform, column-major 4x4 (m); the product of `F_T_NE` and `NE_T_EE`.
82    pub F_T_EE: [f64; 16],
83    /// Flange-to-nominal-end-effector transform as configured in Desk, column-major 4x4 (m).
84    pub F_T_NE: [f64; 16],
85    /// Nominal-end-effector-to-end-effector transform set with [`crate::Robot::set_ee`], column-major 4x4 (m).
86    pub NE_T_EE: [f64; 16],
87    /// End-effector-to-stiffness-frame transform set with [`crate::Robot::set_k`], column-major 4x4 (m).
88    pub EE_T_K: [f64; 16],
89    /// Mass of the end effector, in kg.
90    pub m_ee: f64,
91    /// Inertia matrix of the end effector about its centre of mass, column-major 3x3, in kg*m^2.
92    pub I_ee: [f64; 9],
93    /// Centre of mass of the end effector in flange frame, in m.
94    pub F_x_Cee: [f64; 3],
95    /// Mass of the external load, in kg.
96    pub m_load: f64,
97    /// Inertia matrix of the external load about its centre of mass, column-major 3x3, in kg*m^2.
98    pub I_load: [f64; 9],
99    /// Centre of mass of the external load in flange frame, in m.
100    pub F_x_Cload: [f64; 3],
101    /// Combined mass of end effector and load, in kg.
102    pub m_total: f64,
103    /// Combined inertia matrix of end effector and load, column-major 3x3, in kg*m^2.
104    pub I_total: [f64; 9],
105    /// Combined centre of mass of end effector and load in flange frame, in m.
106    pub F_x_Ctotal: [f64; 3],
107    /// Measured elbow configuration: joint-3 position in rad and the sign (+1/-1) of joint 4.
108    pub elbow: [f64; 2],
109    /// Desired elbow configuration, same layout as [`RobotState::elbow`].
110    pub elbow_d: [f64; 2],
111    /// Commanded elbow configuration, same layout as [`RobotState::elbow`].
112    pub elbow_c: [f64; 2],
113    /// Commanded elbow velocity: joint-3 velocity in rad/s and 0 for the joint-4 sign.
114    pub delbow_c: [f64; 2],
115    /// Commanded elbow acceleration: joint-3 acceleration in rad/s^2 and 0 for the joint-4 sign.
116    pub ddelbow_c: [f64; 2],
117    /// Measured link-side joint torques, in Nm.
118    pub tau_J: [f64; 7],
119    /// Desired link-side joint torques without gravity, in Nm. Rate-limit torque commands against this.
120    pub tau_J_d: [f64; 7],
121    /// Derivative of the measured joint torques, in Nm/s.
122    pub dtau_J: [f64; 7],
123    /// Measured joint positions, in rad.
124    pub q: [f64; 7],
125    /// Desired joint positions, in rad.
126    pub q_d: [f64; 7],
127    /// Measured joint velocities, in rad/s.
128    pub dq: [f64; 7],
129    /// Desired joint velocities, in rad/s.
130    pub dq_d: [f64; 7],
131    /// Desired joint accelerations, in rad/s^2.
132    pub ddq_d: [f64; 7],
133    /// Per-joint contact indicator: 1.0 where a contact was detected, else 0.0.
134    pub joint_contact: [f64; 7],
135    /// Cartesian contact indicator in the stiffness frame (x, y, z, roll, pitch, yaw): 1.0 or 0.0.
136    pub cartesian_contact: [f64; 6],
137    /// Per-joint collision indicator: 1.0 where a collision was detected, else 0.0.
138    pub joint_collision: [f64; 7],
139    /// Cartesian collision indicator in the stiffness frame (x, y, z, roll, pitch, yaw): 1.0 or 0.0.
140    pub cartesian_collision: [f64; 6],
141    /// Filtered external torque estimate, in Nm.
142    pub tau_ext_hat_filtered: [f64; 7],
143    /// Estimated external wrench on the stiffness frame, expressed in base frame: forces in N, torques in Nm.
144    pub O_F_ext_hat_K: [f64; 6],
145    /// Estimated external wrench on the stiffness frame, expressed in the stiffness frame: forces in N, torques in Nm.
146    pub K_F_ext_hat_K: [f64; 6],
147    /// Desired end-effector twist in base frame: linear m/s then angular rad/s.
148    pub O_dP_EE_d: [f64; 6],
149    /// Gravity vector in base frame, in m/s^2 (the robot's own measurement of which way is down).
150    pub O_ddP_O: [f64; 3],
151    /// Last commanded end-effector pose in base frame, column-major 4x4 (m).
152    pub O_T_EE_c: [f64; 16],
153    /// Last commanded end-effector twist in base frame: linear m/s then angular rad/s.
154    pub O_dP_EE_c: [f64; 6],
155    /// Last commanded end-effector acceleration in base frame: linear m/s^2 then angular rad/s^2.
156    pub O_ddP_EE_c: [f64; 6],
157    /// Motor-side joint positions, in rad.
158    pub theta: [f64; 7],
159    /// Motor-side joint velocities, in rad/s.
160    pub dtheta: [f64; 7],
161    /// Raw readings of the six top accelerometers, in m/s^2 (FCI v10 only; zero on an FER).
162    pub accelerometer_top: [[f64; 3]; 6],
163    /// Raw readings of the six bottom accelerometers, in m/s^2 (FCI v10 only; zero on an FER).
164    pub accelerometer_bottom: [[f64; 3]; 6],
165    /// Errors currently active on the robot.
166    pub current_errors: Errors,
167    /// Errors that aborted the last motion (`reflex_reason`).
168    pub last_motion_errors: Errors,
169    /// Fraction of the last 100 cycles whose command the robot accepted, in [0, 1].
170    pub control_command_success_rate: f64,
171    /// Current robot mode.
172    pub robot_mode: RobotMode,
173    /// Strictly monotonic robot timestamp of this state.
174    pub time: Duration,
175}
176
177impl Default for RobotState {
178    /// Identity transforms, zero everything else, `robot_mode = Other`, gravity along -z in
179    /// `O_ddP_O` as the robot reports it.
180    fn default() -> Self {
181        RobotState {
182            O_T_EE: IDENTITY_TRANSFORM,
183            O_T_EE_d: IDENTITY_TRANSFORM,
184            F_T_EE: IDENTITY_TRANSFORM,
185            F_T_NE: IDENTITY_TRANSFORM,
186            NE_T_EE: IDENTITY_TRANSFORM,
187            EE_T_K: IDENTITY_TRANSFORM,
188            m_ee: 0.0,
189            I_ee: [0.0; 9],
190            F_x_Cee: [0.0; 3],
191            m_load: 0.0,
192            I_load: [0.0; 9],
193            F_x_Cload: [0.0; 3],
194            m_total: 0.0,
195            I_total: [0.0; 9],
196            F_x_Ctotal: [0.0; 3],
197            elbow: [0.0; 2],
198            elbow_d: [0.0; 2],
199            elbow_c: [0.0; 2],
200            delbow_c: [0.0; 2],
201            ddelbow_c: [0.0; 2],
202            tau_J: [0.0; 7],
203            tau_J_d: [0.0; 7],
204            dtau_J: [0.0; 7],
205            q: [0.0; 7],
206            q_d: [0.0; 7],
207            dq: [0.0; 7],
208            dq_d: [0.0; 7],
209            ddq_d: [0.0; 7],
210            joint_contact: [0.0; 7],
211            cartesian_contact: [0.0; 6],
212            joint_collision: [0.0; 7],
213            cartesian_collision: [0.0; 6],
214            tau_ext_hat_filtered: [0.0; 7],
215            O_F_ext_hat_K: [0.0; 6],
216            K_F_ext_hat_K: [0.0; 6],
217            O_dP_EE_d: [0.0; 6],
218            O_ddP_O: [0.0, 0.0, -9.81],
219            O_T_EE_c: IDENTITY_TRANSFORM,
220            O_dP_EE_c: [0.0; 6],
221            O_ddP_EE_c: [0.0; 6],
222            theta: [0.0; 7],
223            dtheta: [0.0; 7],
224            accelerometer_top: [[0.0; 3]; 6],
225            accelerometer_bottom: [[0.0; 3]; 6],
226            current_errors: Errors::default(),
227            last_motion_errors: Errors::default(),
228            control_command_success_rate: 0.0,
229            robot_mode: RobotMode::Other,
230            time: Duration::default(),
231        }
232    }
233}
234
235/// Combines the centres of mass of the end effector and the load.
236///
237/// Port of `franka::combineCenterOfMass` (`src/load_calculations.cpp`, libfranka 0.21.2): the
238/// mass-weighted mean, or all zeros when the combined mass is not positive.
239pub fn combine_center_of_mass(
240    m_ee: f64,
241    F_x_Cee: &[f64; 3],
242    m_load: f64,
243    F_x_Cload: &[f64; 3],
244) -> [f64; 3] {
245    let mut F_x_Ctotal = [0.0; 3];
246    if (m_ee + m_load) > 0.0 {
247        for i in 0..3 {
248            F_x_Ctotal[i] = (m_ee * F_x_Cee[i] + m_load * F_x_Cload[i]) / (m_ee + m_load);
249        }
250    }
251    F_x_Ctotal
252}
253
254/// Skew-symmetric matrix of a vector (`franka::skewSymmetricMatrixFromVector`).
255fn skew_symmetric(v: &[f64; 3]) -> nalgebra::Matrix3<f64> {
256    nalgebra::Matrix3::new(0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0)
257}
258
259/// Combines the inertia tensors of the end effector and the load about the combined centre of
260/// mass.
261///
262/// Port of `franka::combineInertiaTensor` (`src/load_calculations.cpp`, libfranka 0.21.2). Both
263/// tensors are shifted to flange coordinates with the parallel-axis theorem, added, and shifted
264/// back to the combined centre of mass. A body with zero mass contributes nothing, and a zero
265/// total mass yields a zero tensor. All tensors are column-major `[f64; 9]`.
266#[allow(clippy::too_many_arguments)]
267pub fn combine_inertia_tensor(
268    m_ee: f64,
269    F_x_Cee: &[f64; 3],
270    I_ee: &[f64; 9],
271    m_load: f64,
272    F_x_Cload: &[f64; 3],
273    I_load: &[f64; 9],
274    m_total: f64,
275    F_x_Ctotal: &[f64; 3],
276) -> [f64; 9] {
277    if m_total == 0.0 {
278        return [0.0; 9];
279    }
280
281    let mut inertia_ee = nalgebra::Matrix3::from_column_slice(I_ee);
282    let mut inertia_load = nalgebra::Matrix3::from_column_slice(I_load);
283    if m_ee == 0.0 {
284        inertia_ee = nalgebra::Matrix3::zeros();
285    }
286    if m_load == 0.0 {
287        inertia_load = nalgebra::Matrix3::zeros();
288    }
289
290    let skew_ee = skew_symmetric(F_x_Cee);
291    let skew_load = skew_symmetric(F_x_Cload);
292    let skew_total = skew_symmetric(F_x_Ctotal);
293
294    let inertia_ee_flange = inertia_ee - m_ee * (skew_ee * skew_ee);
295    let inertia_load_flange = inertia_load - m_load * (skew_load * skew_load);
296    let inertia_total =
297        inertia_ee_flange + inertia_load_flange + m_total * (skew_total * skew_total);
298
299    let mut out = [0.0; 9];
300    out.copy_from_slice(inertia_total.as_slice());
301    out
302}
303
304impl RobotState {
305    /// Widens a wire state to the public `f64` state.
306    ///
307    /// Port of `franka::convertRobotState` (`src/robot_impl.cpp`, libfranka 0.21.2): every
308    /// `float` becomes a `double`, `m_total`/`F_x_Ctotal`/`I_total` are derived from the end
309    /// effector and load properties, `time` is the `message_id` read as milliseconds, and the
310    /// state's `errors`/`reflex_reason` become `current_errors`/`last_motion_errors`. An
311    /// unknown `robot_mode` byte maps to [`RobotMode::Other`], matching the C++ switch whose
312    /// result is pre-initialised to `kOther`.
313    pub fn from_wire(w: &crate::wire::robot::RobotState) -> RobotState {
314        use crate::wire::{accel_to_f64, f32s_to_f64};
315
316        let m_ee = w.m_ee.get() as f64;
317        let m_load = w.m_load.get() as f64;
318        let I_ee = f32s_to_f64(&w.I_ee);
319        let I_load = f32s_to_f64(&w.I_load);
320        let F_x_Cee = f32s_to_f64(&w.F_x_Cee);
321        let F_x_Cload = f32s_to_f64(&w.F_x_Cload);
322
323        // `robot_impl.cpp:516` is `converted.m_total = robot_state.m_ee + robot_state.m_load;`
324        // over two `float`s: the sum is computed in `float` and only then widened, which is not
325        // the same value as adding the two widened doubles.
326        let m_total = f64::from(w.m_ee.get() + w.m_load.get());
327        let F_x_Ctotal = combine_center_of_mass(m_ee, &F_x_Cee, m_load, &F_x_Cload);
328        let I_total = combine_inertia_tensor(
329            m_ee,
330            &F_x_Cee,
331            &I_ee,
332            m_load,
333            &F_x_Cload,
334            &I_load,
335            m_total,
336            &F_x_Ctotal,
337        );
338
339        RobotState {
340            O_T_EE: f32s_to_f64(&w.O_T_EE),
341            O_T_EE_d: f32s_to_f64(&w.O_T_EE_d),
342            F_T_EE: f32s_to_f64(&w.F_T_EE),
343            F_T_NE: f32s_to_f64(&w.F_T_NE),
344            NE_T_EE: f32s_to_f64(&w.NE_T_EE),
345            EE_T_K: f32s_to_f64(&w.EE_T_K),
346            m_ee,
347            I_ee,
348            F_x_Cee,
349            m_load,
350            I_load,
351            F_x_Cload,
352            m_total,
353            I_total,
354            F_x_Ctotal,
355            elbow: f32s_to_f64(&w.elbow),
356            elbow_d: f32s_to_f64(&w.elbow_d),
357            elbow_c: f32s_to_f64(&w.elbow_c),
358            delbow_c: f32s_to_f64(&w.delbow_c),
359            ddelbow_c: f32s_to_f64(&w.ddelbow_c),
360            tau_J: f32s_to_f64(&w.tau_J),
361            tau_J_d: f32s_to_f64(&w.tau_J_d),
362            dtau_J: f32s_to_f64(&w.dtau_J),
363            q: f32s_to_f64(&w.q),
364            q_d: f32s_to_f64(&w.q_d),
365            dq: f32s_to_f64(&w.dq),
366            dq_d: f32s_to_f64(&w.dq_d),
367            ddq_d: f32s_to_f64(&w.ddq_d),
368            joint_contact: f32s_to_f64(&w.joint_contact),
369            cartesian_contact: f32s_to_f64(&w.cartesian_contact),
370            joint_collision: f32s_to_f64(&w.joint_collision),
371            cartesian_collision: f32s_to_f64(&w.cartesian_collision),
372            tau_ext_hat_filtered: f32s_to_f64(&w.tau_ext_hat_filtered),
373            O_F_ext_hat_K: f32s_to_f64(&w.O_F_ext_hat_K),
374            K_F_ext_hat_K: f32s_to_f64(&w.K_F_ext_hat_K),
375            O_dP_EE_d: f32s_to_f64(&w.O_dP_EE_d),
376            O_ddP_O: f32s_to_f64(&w.O_ddP_O),
377            O_T_EE_c: f32s_to_f64(&w.O_T_EE_c),
378            O_dP_EE_c: f32s_to_f64(&w.O_dP_EE_c),
379            O_ddP_EE_c: f32s_to_f64(&w.O_ddP_EE_c),
380            theta: f32s_to_f64(&w.theta),
381            dtheta: f32s_to_f64(&w.dtheta),
382            accelerometer_top: accel_to_f64(&w.accelerometer_top),
383            accelerometer_bottom: accel_to_f64(&w.accelerometer_bottom),
384            current_errors: Errors::from(w.errors),
385            last_motion_errors: Errors::from(w.reflex_reason),
386            control_command_success_rate: w.control_command_success_rate.get() as f64,
387            robot_mode: RobotMode::from_u8(w.robot_mode).unwrap_or(RobotMode::Other),
388            time: Duration::from_millis(w.message_id.get()),
389        }
390    }
391
392    /// Widens an FCI v5 wire state to the public state.
393    ///
394    /// Port of `franka::convertRobotState` (`src/robot_impl.cpp`, libfranka 0.9.2). The FER
395    /// streams `double` already, so nothing is widened; `m_total`/`F_x_Ctotal`/`I_total` are
396    /// combined exactly as on FCI v10, and `time` is the `message_id` read as milliseconds.
397    ///
398    /// FCI v5 has no accelerometer arrays in its state (`rbk_types.h:38-85`, libfranka 0.9.2),
399    /// so `accelerometer_top` and `accelerometer_bottom` are zero. An unknown `robot_mode` byte
400    /// maps to [`RobotMode::Other`], matching the C++ switch whose result is pre-initialised to
401    /// `kOther`.
402    pub fn from_wire_v5(w: &crate::wire::robot::v5::RobotState) -> RobotState {
403        use crate::wire::f64s_to_f64;
404
405        let m_ee = w.m_ee.get();
406        let m_load = w.m_load.get();
407        let I_ee = f64s_to_f64(&w.I_ee);
408        let I_load = f64s_to_f64(&w.I_load);
409        let F_x_Cee = f64s_to_f64(&w.F_x_Cee);
410        let F_x_Cload = f64s_to_f64(&w.F_x_Cload);
411
412        let m_total = m_ee + m_load;
413        let F_x_Ctotal = combine_center_of_mass(m_ee, &F_x_Cee, m_load, &F_x_Cload);
414        let I_total = combine_inertia_tensor(
415            m_ee,
416            &F_x_Cee,
417            &I_ee,
418            m_load,
419            &F_x_Cload,
420            &I_load,
421            m_total,
422            &F_x_Ctotal,
423        );
424
425        RobotState {
426            O_T_EE: f64s_to_f64(&w.O_T_EE),
427            O_T_EE_d: f64s_to_f64(&w.O_T_EE_d),
428            F_T_EE: f64s_to_f64(&w.F_T_EE),
429            F_T_NE: f64s_to_f64(&w.F_T_NE),
430            NE_T_EE: f64s_to_f64(&w.NE_T_EE),
431            EE_T_K: f64s_to_f64(&w.EE_T_K),
432            m_ee,
433            I_ee,
434            F_x_Cee,
435            m_load,
436            I_load,
437            F_x_Cload,
438            m_total,
439            I_total,
440            F_x_Ctotal,
441            elbow: f64s_to_f64(&w.elbow),
442            elbow_d: f64s_to_f64(&w.elbow_d),
443            elbow_c: f64s_to_f64(&w.elbow_c),
444            delbow_c: f64s_to_f64(&w.delbow_c),
445            ddelbow_c: f64s_to_f64(&w.ddelbow_c),
446            tau_J: f64s_to_f64(&w.tau_J),
447            tau_J_d: f64s_to_f64(&w.tau_J_d),
448            dtau_J: f64s_to_f64(&w.dtau_J),
449            q: f64s_to_f64(&w.q),
450            q_d: f64s_to_f64(&w.q_d),
451            dq: f64s_to_f64(&w.dq),
452            dq_d: f64s_to_f64(&w.dq_d),
453            ddq_d: f64s_to_f64(&w.ddq_d),
454            joint_contact: f64s_to_f64(&w.joint_contact),
455            cartesian_contact: f64s_to_f64(&w.cartesian_contact),
456            joint_collision: f64s_to_f64(&w.joint_collision),
457            cartesian_collision: f64s_to_f64(&w.cartesian_collision),
458            tau_ext_hat_filtered: f64s_to_f64(&w.tau_ext_hat_filtered),
459            O_F_ext_hat_K: f64s_to_f64(&w.O_F_ext_hat_K),
460            K_F_ext_hat_K: f64s_to_f64(&w.K_F_ext_hat_K),
461            O_dP_EE_d: f64s_to_f64(&w.O_dP_EE_d),
462            O_ddP_O: f64s_to_f64(&w.O_ddP_O),
463            O_T_EE_c: f64s_to_f64(&w.O_T_EE_c),
464            O_dP_EE_c: f64s_to_f64(&w.O_dP_EE_c),
465            O_ddP_EE_c: f64s_to_f64(&w.O_ddP_EE_c),
466            theta: f64s_to_f64(&w.theta),
467            dtheta: f64s_to_f64(&w.dtheta),
468            accelerometer_top: [[0.0; 3]; 6],
469            accelerometer_bottom: [[0.0; 3]; 6],
470            current_errors: Errors::from(w.errors),
471            last_motion_errors: Errors::from(w.reflex_reason),
472            control_command_success_rate: w.control_command_success_rate.get(),
473            robot_mode: RobotMode::from_u8(w.robot_mode).unwrap_or(RobotMode::Other),
474            time: Duration::from_millis(w.message_id.get()),
475        }
476    }
477}
478
479#[cfg(test)]
480mod tests;
franka/robot_state/mod.rs

franka/robot_state/
mod.rs
