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
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;
#[derive(Debug, Copy, Clone)]
pub struct RobotCommandLog {
pub joint_positions: JointPositions,
pub joint_velocities: JointVelocities,
pub cartesian_pose: CartesianPose,
pub cartesian_velocities: CartesianVelocities,
pub torques: Torques,
}
#[derive(Debug, Clone)]
pub struct Record {
pub state: RobotState,
pub command: RobotCommandLog,
}
impl Record {
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
}
}