franka/robot_state/
mod.rs

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

franka/robot_state/
mod.rs
