diff --git a/crates/control/src/motion/head_motion.rs b/crates/control/src/motion/head_motion.rs index ef5b2fa80b..81912d7388 100644 --- a/crates/control/src/motion/head_motion.rs +++ b/crates/control/src/motion/head_motion.rs @@ -2,6 +2,7 @@ use std::f32::consts::PI; use color_eyre::Result; use context_attribute::context; +use filtering::low_pass_filter::LowPassFilter; use framework::MainOutput; use serde::{Deserialize, Serialize}; use types::{ @@ -16,6 +17,7 @@ use types::{ #[derive(Default, Deserialize, Serialize)] pub struct HeadMotion { last_positions: HeadJoints, + lowpass_filter: LowPassFilter>, } #[context] @@ -28,6 +30,7 @@ pub struct CycleContext { maximum_velocity: Parameter, "head_motion.maximum_velocity">, outer_maximum_pitch: Parameter, outer_yaw: Parameter, + injected_head_joints: Parameter>, "head_motion.injected_head_joints?">, look_around: Input, "look_around">, look_at: Input, "look_at">, @@ -48,10 +51,22 @@ impl HeadMotion { pub fn new(_context: CreationContext) -> Result { Ok(Self { last_positions: Default::default(), + lowpass_filter: LowPassFilter::with_smoothing_factor(Default::default(), 0.075), }) } pub fn cycle(&mut self, context: CycleContext) -> Result { + if let Some(injected_head_joints) = context.injected_head_joints.copied() { + self.lowpass_filter.update(injected_head_joints); + + return Ok(MainOutputs { + head_joints_command: MotorCommands { + positions: self.lowpass_filter.state(), + stiffnesses: HeadJoints::fill(0.8), + } + .into(), + }); + } if context.motion_selection.dispatching_motion.is_some() { return Ok(MainOutputs { head_joints_command: MotorCommands { diff --git a/crates/types/src/players.rs b/crates/types/src/players.rs index 02af061ae7..65cf7e8c6c 100644 --- a/crates/types/src/players.rs +++ b/crates/types/src/players.rs @@ -58,24 +58,24 @@ impl IndexMut for Players { } } +fn get_penalty(team_state: &TeamState, index: usize) -> Option { + team_state + .players + .get(index) + .map(|player| player.penalty) + .unwrap_or_default() +} + impl From for Players> { fn from(team_state: TeamState) -> Self { Self { - one: team_state.players[0].penalty, - two: team_state.players[1].penalty, - three: team_state.players[2].penalty, - four: team_state.players[3].penalty, - five: team_state.players[4].penalty, - six: team_state - .players - .get(5) - .map(|player| player.penalty) - .unwrap_or_default(), - seven: team_state - .players - .get(6) - .map(|player| player.penalty) - .unwrap_or_default(), + one: get_penalty(&team_state, 0), + two: get_penalty(&team_state, 1), + three: get_penalty(&team_state, 2), + four: get_penalty(&team_state, 3), + five: get_penalty(&team_state, 4), + six: get_penalty(&team_state, 5), + seven: get_penalty(&team_state, 6), } } } diff --git a/etc/parameters/default.json b/etc/parameters/default.json index dae71ade48..9a74681a3e 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -999,7 +999,8 @@ }, "outer_maximum_pitch": 0.0, "inner_maximum_pitch": 0.61, - "outer_yaw": 1.3 + "outer_yaw": 1.3, + "injected_head_joints": null }, "look_at": { "glance_angle": 0.25, diff --git a/tools/twix/src/panels/remote.rs b/tools/twix/src/panels/remote.rs index 8b05fefb27..337d065955 100644 --- a/tools/twix/src/panels/remote.rs +++ b/tools/twix/src/panels/remote.rs @@ -1,10 +1,13 @@ -use std::sync::Arc; +use std::{ + sync::Arc, + time::{Duration, SystemTime}, +}; use communication::messages::TextOrBinary; use eframe::egui::Widget; use gilrs::{Axis, Button, Gamepad, GamepadId, Gilrs}; use serde_json::{json, Value}; -use types::step_plan::Step; +use types::{joints::head::HeadJoints, step_plan::Step}; use crate::{nao::Nao, panel::Panel}; @@ -13,6 +16,7 @@ pub struct RemotePanel { gilrs: Gilrs, active_gamepad: Option, enabled: bool, + last_update: SystemTime, } impl Panel for RemotePanel { @@ -28,6 +32,7 @@ impl Panel for RemotePanel { gilrs, active_gamepad, enabled, + last_update: SystemTime::now(), } } @@ -41,27 +46,43 @@ fn get_axis_value(gamepad: Gamepad, axis: Axis) -> Option { } impl RemotePanel { + fn reset(&self) { + self.update_step(Value::Null); + self.update_look_at_angle(Value::Null); + } + fn update_step(&self, step: Value) { self.nao.write( "parameters.step_planner.injected_step", TextOrBinary::Text(step), ) } + + fn update_look_at_angle(&self, joints: Value) { + self.nao.write( + "parameters.head_motion.injected_head_joints", + TextOrBinary::Text(joints), + ) + } } impl Widget for &mut RemotePanel { fn ui(self, ui: &mut eframe::egui::Ui) -> eframe::egui::Response { + const UPDATE_DELAY: Duration = Duration::from_millis(100); + const HEAD_PITCH_SCALE: f32 = 1.0; + const HEAD_YAW_SCALE: f32 = 1.0; + self.gilrs.inc(); if ui.checkbox(&mut self.enabled, "Enabled (Start)").changed() { - self.update_step(Value::Null); + self.reset(); }; while let Some(event) = self.gilrs.next_event() { if let gilrs::EventType::ButtonPressed(Button::Start, _) = event.event { self.enabled = !self.enabled; if !self.enabled { - self.update_step(Value::Null) + self.reset(); } }; self.active_gamepad = Some(event.id); @@ -70,10 +91,26 @@ impl Widget for &mut RemotePanel { if let Some(gamepad) = self.active_gamepad.map(|id| self.gilrs.gamepad(id)) { let right = get_axis_value(gamepad, Axis::LeftStickX).unwrap_or(0.0); let forward = get_axis_value(gamepad, Axis::LeftStickY).unwrap_or(0.0); - let turn_right = get_axis_value(gamepad, Axis::RightStickX).unwrap_or(0.0); let left = -right; - let turn = -turn_right; + + let turn_right = gamepad + .button_data(Button::RightTrigger2) + .map(|button| button.value()) + .unwrap_or_default(); + let turn_left = gamepad + .button_data(Button::LeftTrigger2) + .map(|button| button.value()) + .unwrap_or_default(); + let turn = turn_left - turn_right; + + let head_pitch = get_axis_value(gamepad, Axis::RightStickY).unwrap_or(0.0); + let head_yaw = -get_axis_value(gamepad, Axis::RightStickX).unwrap_or(0.0); + + let injected_head_joints = HeadJoints { + yaw: head_yaw * HEAD_YAW_SCALE, + pitch: head_pitch * HEAD_PITCH_SCALE, + }; let step = Step { forward, @@ -82,9 +119,25 @@ impl Widget for &mut RemotePanel { }; if self.enabled { - self.update_step(serde_json::to_value(step).unwrap()); + let now = SystemTime::now(); + if now + .duration_since(self.last_update) + .expect("Time ran backwards") + > UPDATE_DELAY + { + self.last_update = now; + self.update_step(serde_json::to_value(step).unwrap()); + self.update_look_at_angle(serde_json::to_value(injected_head_joints).unwrap()); + } } - ui.label(&format!("{:#?}", step)) + + ui.vertical(|ui| { + let label_1 = ui.label(&format!("{:#?}", step)); + let label_2 = ui.label(&format!("{:#?}", injected_head_joints)); + + label_1.union(label_2) + }) + .inner } else { ui.label("No controller found") }