1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

// Copyright (c) 2021 Marco Boneberger
// Licensed under the EUPL-1.2-or-later

//! Contains the logging type definitions for [`ControlException`](`crate::exception::FrankaException::ControlException`)
use crate::robot::control_types::{
    CartesianPose, CartesianVelocities, JointPositions, JointVelocities, Torques,
};
use crate::robot::robot_state::RobotState;
use crate::robot::types::RobotCommand;
use std::collections::VecDeque;

/// Command sent to the robot. Structure used only for logging purposes.
#[derive(Debug, Copy, Clone)]
pub struct RobotCommandLog {
    /// sent joint positions.
    pub joint_positions: JointPositions,

    /// sent joint velocities.
    pub joint_velocities: JointVelocities,

    /// sent cartesian positions.
    pub cartesian_pose: CartesianPose,

    /// sent cartesian velocities.
    pub cartesian_velocities: CartesianVelocities,

    /// sent torques.
    pub torques: Torques,
}

/// One row of the log contains a robot command of timestamp n and a
/// corresponding robot state of timestamp n+1.
/// Provided by the [`ControlException`](`crate::exception::FrankaException::ControlException`).
#[derive(Debug, Clone)]
pub struct Record {
    /// Robot state of timestamp n+1.
    pub state: RobotState,
    /// Robot command of timestamp n, after rate limiting (if activated).
    pub command: RobotCommandLog,
}

impl Record {
    /// creates a string representation based on the debug formatter
    pub fn log(&self) -> String {
        format!("{:?}", self.clone())
    }
}

pub(crate) struct Logger {
    states: VecDeque<RobotState>,
    commands: VecDeque<RobotCommand>,
    ring_front: usize,
    ring_size: usize,
    log_size: usize,
}

impl Logger {
    pub fn new(log_size: usize) -> Self {
        Logger {
            states: VecDeque::with_capacity(log_size),
            commands: VecDeque::with_capacity(log_size),
            ring_front: 0,
            ring_size: 0,
            log_size,
        }
    }
    pub fn log(&mut self, state: &RobotState, command: &RobotCommand) {
        self.states.push_back(state.clone());
        self.commands.push_back(*command);
        self.ring_front = (self.ring_front + 1) % self.log_size;
        if self.ring_size == self.log_size {
            self.states.pop_front();
            self.commands.pop_front();
        }
        self.ring_size = usize::min(self.log_size, self.ring_size + 1);
    }
    pub fn flush(&mut self) -> Vec<Record> {
        let mut out: Vec<Record> = Vec::new();
        for i in 0..self.ring_size {
            let index = (self.ring_front + i) % self.ring_size;
            let cmd = self.commands.get(index).unwrap();
            let command = RobotCommandLog {
                joint_positions: JointPositions::new(cmd.motion.q_c),
                joint_velocities: JointVelocities::new(cmd.motion.dq_c),
                cartesian_pose: CartesianPose::new(cmd.motion.O_T_EE_c, None),
                cartesian_velocities: CartesianVelocities::new(cmd.motion.O_dP_EE_c, None),
                torques: Torques::new(cmd.control.tau_J_d),
            };
            let record = Record {
                state: self.states.get(index).unwrap().clone(),
                command,
            };
            out.push(record);
        }
        self.ring_front = 0;
        self.ring_size = 0;
        out
    }
}