From 249a72e145eb75ff2b3a1cf4e5947337d8e693d3 Mon Sep 17 00:00:00 2001 From: larskna <143598820+larskna@users.noreply.github.com> Date: Mon, 15 Jul 2024 17:08:43 +0200 Subject: [PATCH 1/3] add the math and the motions left and right wideStance --- crates/control/src/behavior/defend.rs | 24 +- .../src/motion/dispatching_interpolator.rs | 4 + crates/control/src/motion/mod.rs | 2 + crates/control/src/motion/motion_selector.rs | 21 +- .../src/motion/motor_commands_collector.rs | 28 ++ crates/control/src/motion/wide_stance_left.rs | 77 +++ .../control/src/motion/wide_stance_right.rs | 77 +++ crates/hulk_manifest/src/lib.rs | 2 + crates/types/src/motion_command.rs | 7 +- crates/types/src/motion_selection.rs | 12 + etc/motions/wide_stance_left.json | 457 ++++++++++++++++++ 11 files changed, 702 insertions(+), 9 deletions(-) create mode 100644 crates/control/src/motion/wide_stance_left.rs create mode 100644 crates/control/src/motion/wide_stance_right.rs create mode 100644 etc/motions/wide_stance_left.json diff --git a/crates/control/src/behavior/defend.rs b/crates/control/src/behavior/defend.rs index 236a1b2d14..1da264a81d 100644 --- a/crates/control/src/behavior/defend.rs +++ b/crates/control/src/behavior/defend.rs @@ -8,7 +8,7 @@ use spl_network_messages::{GamePhase, SubState, Team}; use types::{ field_dimensions::FieldDimensions, filtered_game_controller_state::FilteredGameControllerState, - motion_command::{MotionCommand, WalkSpeed}, + motion_command::{JumpDirection, MotionCommand, WalkSpeed}, parameters::{RolePositionsParameters, WideStanceParameters}, path_obstacles::PathObstacle, support_foot::Side, @@ -72,10 +72,24 @@ impl<'cycle> Defend<'cycle> { let horizontal_distance_to_intersection = position.y() - position.x() / velocity.x() * velocity.y(); - if horizontal_distance_to_intersection.abs() < wide_stance_paramters.action_radius { - Some(MotionCommand::WideStance) - } else { - None + match ( + (-wide_stance_paramters.action_radius..=wide_stance_paramters.action_radius) + .contains(&horizontal_distance_to_intersection), + (wide_stance_paramters.action_radius..0.6) + .contains(&horizontal_distance_to_intersection), + (-0.6..-wide_stance_paramters.action_radius) + .contains(&horizontal_distance_to_intersection), + ) { + (true, _, _) => Some(MotionCommand::WideStance { + direction: JumpDirection::Center, + }), + (false, true, _) => Some(MotionCommand::WideStance { + direction: JumpDirection::Left, + }), + (false, _, true) => Some(MotionCommand::WideStance { + direction: JumpDirection::Right, + }), + (false, false, false) => None, } } diff --git a/crates/control/src/motion/dispatching_interpolator.rs b/crates/control/src/motion/dispatching_interpolator.rs index 21cdcf62bb..c37b770a24 100644 --- a/crates/control/src/motion/dispatching_interpolator.rs +++ b/crates/control/src/motion/dispatching_interpolator.rs @@ -38,6 +38,8 @@ pub struct CycleContext { stand_up_front_positions: Input, "stand_up_front_positions">, stand_up_sitting_positions: Input, "stand_up_sitting_positions">, wide_stance_positions: Input, "wide_stance_positions">, + wide_stance_left_positions: Input, "wide_stance_left_positions">, + wide_stance_right_positions: Input, "wide_stance_right_positions">, walk_motor_commands: Input>, "walk_motor_commands">, initial_pose: Parameter, "initial_pose">, @@ -105,6 +107,8 @@ impl DispatchingInterpolator { MotionType::StandUpFront => *context.stand_up_front_positions, MotionType::StandUpSitting => *context.stand_up_sitting_positions, MotionType::WideStance => *context.wide_stance_positions, + MotionType::WideStanceLeft => *context.wide_stance_left_positions, + MotionType::WideStanceRight => *context.wide_stance_right_positions, MotionType::CenterJump => *context.center_jump_positions, MotionType::Unstiff => panic!("Dispatching Unstiff doesn't make sense"), MotionType::Animation => context.animation_commands.positions, diff --git a/crates/control/src/motion/mod.rs b/crates/control/src/motion/mod.rs index 6c142d92b1..758514df73 100644 --- a/crates/control/src/motion/mod.rs +++ b/crates/control/src/motion/mod.rs @@ -23,3 +23,5 @@ pub mod step_planner; pub mod walk_manager; pub mod walking_engine; pub mod wide_stance; +pub mod wide_stance_left; +pub mod wide_stance_right; diff --git a/crates/control/src/motion/motion_selector.rs b/crates/control/src/motion/motion_selector.rs index 41a077e06e..30ea245b05 100644 --- a/crates/control/src/motion/motion_selector.rs +++ b/crates/control/src/motion/motion_selector.rs @@ -87,7 +87,12 @@ fn motion_type_from_command(command: &MotionCommand) -> MotionType { Kind::FacingUp => MotionType::StandUpBack, Kind::Sitting => MotionType::StandUpSitting, }, - MotionCommand::WideStance => MotionType::WideStance, + MotionCommand::WideStance { direction } => match direction { + JumpDirection::Left => MotionType::WideStanceLeft, + JumpDirection::Right => MotionType::WideStanceRight, + JumpDirection::Center => MotionType::WideStance, + }, + MotionCommand::Unstiff => MotionType::Unstiff, MotionCommand::Animation { stiff: false } => MotionType::Animation, MotionCommand::Animation { stiff: true } => MotionType::AnimationStiff, @@ -109,6 +114,13 @@ fn transition_motion( (MotionType::StandUpFront, _, MotionType::FallProtection, _) => MotionType::StandUpFront, (MotionType::StandUpBack, _, MotionType::FallProtection, _) => MotionType::StandUpBack, (MotionType::WideStance, _, MotionType::FallProtection, _) => MotionType::WideStance, + (MotionType::WideStanceLeft, _, MotionType::FallProtection, _) => { + MotionType::WideStanceLeft + } + + (MotionType::WideStanceRight, _, MotionType::FallProtection, _) => { + MotionType::WideStanceRight + } (MotionType::JumpLeft, _, MotionType::FallProtection, _) => MotionType::JumpLeft, (MotionType::JumpRight, _, MotionType::FallProtection, _) => MotionType::JumpRight, (MotionType::CenterJump, _, MotionType::FallProtection, _) => MotionType::CenterJump, @@ -122,7 +134,12 @@ fn transition_motion( MotionType::Dispatching } (_, _, MotionType::FallProtection, _) => MotionType::FallProtection, - (_, _, MotionType::WideStance, _) => MotionType::WideStance, + (MotionType::Walk, _, MotionType::WideStance, _) => MotionType::WideStance, + (MotionType::Walk, _, MotionType::WideStanceRight, _) => MotionType::WideStanceRight, + (MotionType::Walk, _, MotionType::WideStanceLeft, _) => MotionType::WideStanceLeft, + (_, true, MotionType::WideStance, _) => MotionType::WideStance, + (_, true, MotionType::WideStanceRight, _) => MotionType::WideStanceRight, + (_, true, MotionType::WideStanceLeft, _) => MotionType::WideStanceLeft, (_, _, MotionType::CenterJump, _) => MotionType::CenterJump, (MotionType::ArmsUpStand, _, _, false) => MotionType::ArmsUpStand, (MotionType::Dispatching, true, _, _) => to, diff --git a/crates/control/src/motion/motor_commands_collector.rs b/crates/control/src/motion/motor_commands_collector.rs index ab2cc22e64..97dd620b27 100644 --- a/crates/control/src/motion/motor_commands_collector.rs +++ b/crates/control/src/motion/motor_commands_collector.rs @@ -36,6 +36,8 @@ pub struct CycleContext { stand_up_front_positions: Input, "stand_up_front_positions">, stand_up_sitting_positions: Input, "stand_up_sitting_positions">, wide_stance_positions: Input, "wide_stance_positions">, + wide_stance_left_positions: Input, "wide_stance_left_positions">, + wide_stance_right_positions: Input, "wide_stance_right_positions">, center_jump_positions: Input, "center_jump_positions">, walk_motor_commands: Input>, "walk_motor_commands">, cycle_time: Input, @@ -82,6 +84,8 @@ impl MotorCommandCollector { let stand_up_front_positions = context.stand_up_front_positions; let stand_up_sitting_positions = context.stand_up_sitting_positions; let wide_stance_positions = context.wide_stance_positions; + let wide_stance_left_positions = context.wide_stance_left_positions; + let wide_stance_right_positions = context.wide_stance_right_positions; let walk = context.walk_motor_commands; let (positions, stiffnesses) = match motion_selection.current_motion { @@ -191,6 +195,30 @@ impl MotorCommandCollector { }, ), ), + MotionType::WideStanceLeft => ( + *wide_stance_left_positions, + Joints::from_head_and_body( + HeadJoints::fill(*context.stand_up_stiffness_upper_body), + BodyJoints { + left_arm: ArmJoints::fill(*context.stand_up_stiffness_upper_body), + right_arm: ArmJoints::fill(*context.stand_up_stiffness_upper_body), + left_leg: LegJoints::fill(1.0), + right_leg: LegJoints::fill(1.0), + }, + ), + ), + MotionType::WideStanceRight => ( + *wide_stance_right_positions, + Joints::from_head_and_body( + HeadJoints::fill(*context.stand_up_stiffness_upper_body), + BodyJoints { + left_arm: ArmJoints::fill(*context.stand_up_stiffness_upper_body), + right_arm: ArmJoints::fill(*context.stand_up_stiffness_upper_body), + left_leg: LegJoints::fill(1.0), + right_leg: LegJoints::fill(1.0), + }, + ), + ), MotionType::Unstiff => (measured_positions, Joints::fill(0.0)), MotionType::Walk => ( Joints::from_head_and_body(head_joints_command.positions, walk.positions), diff --git a/crates/control/src/motion/wide_stance_left.rs b/crates/control/src/motion/wide_stance_left.rs new file mode 100644 index 0000000000..bfe975cc35 --- /dev/null +++ b/crates/control/src/motion/wide_stance_left.rs @@ -0,0 +1,77 @@ +use color_eyre::Result; +use context_attribute::context; +use framework::MainOutput; +use hardware::PathsInterface; +use motionfile::{MotionFile, MotionInterpolator}; +use serde::{Deserialize, Serialize}; +use std::time::Duration; +use types::{ + condition_input::ConditionInput, + cycle_time::CycleTime, + joints::Joints, + motion_selection::{MotionSafeExits, MotionSelection, MotionType}, +}; + +#[derive(Deserialize, Serialize)] +pub struct WideStanceLeft { + interpolator: MotionInterpolator>, +} + +#[context] +pub struct CreationContext { + hardware_interface: HardwareInterface, +} + +#[context] +pub struct CycleContext { + condition_input: Input, + cycle_time: Input, + motion_selection: Input, + + motion_safe_exits: CyclerState, +} + +#[context] +#[derive(Default)] +pub struct MainOutputs { + pub wide_stance_left_positions: MainOutput>, + pub wide_stance_left_estimated_remaining_duration: MainOutput>, +} + +impl WideStanceLeft { + pub fn new(context: CreationContext) -> Result { + let paths = context.hardware_interface.get_paths(); + Ok(Self { + interpolator: MotionFile::from_path(paths.motions.join("wide_stance_left.json"))? + .try_into()?, + }) + } + + pub fn advance_interpolator(&mut self, context: CycleContext) { + let last_cycle_duration = context.cycle_time.last_cycle_duration; + let condition_input = context.condition_input; + + context.motion_safe_exits[MotionType::WideStanceLeft] = false; + + self.interpolator + .advance_by(last_cycle_duration, condition_input); + + context.motion_safe_exits[MotionType::WideStanceLeft] = self.interpolator.is_finished(); + } + + pub fn cycle(&mut self, context: CycleContext) -> Result { + let wide_stance_left_estimated_remaining_duration = + if let MotionType::WideStanceLeft = context.motion_selection.current_motion { + self.advance_interpolator(context); + Some(self.interpolator.estimated_remaining_duration()) + } else { + self.interpolator.reset(); + None + }; + Ok(MainOutputs { + wide_stance_left_positions: self.interpolator.value().into(), + wide_stance_left_estimated_remaining_duration: + wide_stance_left_estimated_remaining_duration.into(), + }) + } +} diff --git a/crates/control/src/motion/wide_stance_right.rs b/crates/control/src/motion/wide_stance_right.rs new file mode 100644 index 0000000000..c6cbc3ca9d --- /dev/null +++ b/crates/control/src/motion/wide_stance_right.rs @@ -0,0 +1,77 @@ +use color_eyre::Result; +use context_attribute::context; +use framework::MainOutput; +use hardware::PathsInterface; +use motionfile::{MotionFile, MotionInterpolator}; +use serde::{Deserialize, Serialize}; +use std::time::Duration; +use types::{ + condition_input::ConditionInput, + cycle_time::CycleTime, + joints::{mirror::Mirror, Joints}, + motion_selection::{MotionSafeExits, MotionSelection, MotionType}, +}; + +#[derive(Deserialize, Serialize)] +pub struct WideStanceRight { + interpolator: MotionInterpolator>, +} + +#[context] +pub struct CreationContext { + hardware_interface: HardwareInterface, +} + +#[context] +pub struct CycleContext { + condition_input: Input, + cycle_time: Input, + motion_selection: Input, + + motion_safe_exits: CyclerState, +} + +#[context] +#[derive(Default)] +pub struct MainOutputs { + pub wide_stance_right_positions: MainOutput>, + pub wide_stance_right_estimated_remaining_duration: MainOutput>, +} + +impl WideStanceRight { + pub fn new(context: CreationContext) -> Result { + let paths = context.hardware_interface.get_paths(); + Ok(Self { + interpolator: MotionFile::from_path(paths.motions.join("wide_stance_left.json"))? + .try_into()?, + }) + } + + pub fn advance_interpolator(&mut self, context: CycleContext) { + let last_cycle_duration = context.cycle_time.last_cycle_duration; + let condition_input = context.condition_input; + + context.motion_safe_exits[MotionType::WideStanceRight] = false; + + self.interpolator + .advance_by(last_cycle_duration, condition_input); + + context.motion_safe_exits[MotionType::WideStanceRight] = self.interpolator.is_finished(); + } + + pub fn cycle(&mut self, context: CycleContext) -> Result { + let wide_stance_right_estimated_remaining_duration = + if let MotionType::WideStanceRight = context.motion_selection.current_motion { + self.advance_interpolator(context); + Some(self.interpolator.estimated_remaining_duration()) + } else { + self.interpolator.reset(); + None + }; + Ok(MainOutputs { + wide_stance_right_positions: self.interpolator.value().mirrored().into(), + wide_stance_right_estimated_remaining_duration: + wide_stance_right_estimated_remaining_duration.into(), + }) + } +} diff --git a/crates/hulk_manifest/src/lib.rs b/crates/hulk_manifest/src/lib.rs index 09a5afbd2c..5d9430334d 100644 --- a/crates/hulk_manifest/src/lib.rs +++ b/crates/hulk_manifest/src/lib.rs @@ -87,6 +87,8 @@ pub fn collect_hulk_cyclers() -> Result { "control::motion::step_planner", "control::motion::walk_manager", "control::motion::wide_stance", + "control::motion::wide_stance_right", + "control::motion::wide_stance_left", "control::motion::walking_engine", "control::obstacle_filter", "control::odometry", diff --git a/crates/types/src/motion_command.rs b/crates/types/src/motion_command.rs index 9a46c1cf99..3cb7e63998 100644 --- a/crates/types/src/motion_command.rs +++ b/crates/types/src/motion_command.rs @@ -63,7 +63,10 @@ pub enum MotionCommand { StandUp { kind: Kind, }, - WideStance, + WideStance { + direction: JumpDirection, + }, + #[default] Unstiff, Animation { @@ -103,7 +106,7 @@ impl MotionCommand { | MotionCommand::FallProtection { .. } | MotionCommand::Jump { .. } | MotionCommand::StandUp { .. } => None, - MotionCommand::WideStance => None, + MotionCommand::WideStance { .. } => None, } } diff --git a/crates/types/src/motion_selection.rs b/crates/types/src/motion_selection.rs index 2b72a27051..00774da62a 100644 --- a/crates/types/src/motion_selection.rs +++ b/crates/types/src/motion_selection.rs @@ -43,6 +43,8 @@ pub enum MotionType { Unstiff, Walk, WideStance, + WideStanceLeft, + WideStanceRight, } impl Default for MotionType { @@ -72,6 +74,8 @@ pub struct MotionSafeExits { unstiff: bool, walk: bool, wide_stance: bool, + wide_stance_left: bool, + wide_stance_right: bool, } impl MotionSafeExits { @@ -96,6 +100,8 @@ impl MotionSafeExits { unstiff: value, walk: value, wide_stance: value, + wide_stance_left: value, + wide_stance_right: value, } } } @@ -122,6 +128,8 @@ impl Default for MotionSafeExits { unstiff: true, walk: false, wide_stance: false, + wide_stance_left: false, + wide_stance_right: false, } } } @@ -150,6 +158,8 @@ impl Index for MotionSafeExits { MotionType::Unstiff => &self.unstiff, MotionType::Walk => &self.walk, MotionType::WideStance => &self.wide_stance, + MotionType::WideStanceLeft => &self.wide_stance_left, + MotionType::WideStanceRight => &self.wide_stance_right, } } } @@ -176,6 +186,8 @@ impl IndexMut for MotionSafeExits { MotionType::Unstiff => &mut self.unstiff, MotionType::Walk => &mut self.walk, MotionType::WideStance => &mut self.wide_stance, + MotionType::WideStanceLeft => &mut self.wide_stance_left, + MotionType::WideStanceRight => &mut self.wide_stance_right, } } } diff --git a/etc/motions/wide_stance_left.json b/etc/motions/wide_stance_left.json new file mode 100644 index 0000000000..10bc9dcccb --- /dev/null +++ b/etc/motions/wide_stance_left.json @@ -0,0 +1,457 @@ +{ + "interpolation_mode": "linear", + "initial_positions": { + "head": { + "pitch": 0.34176361560821533, + "yaw": 0.6000000238418579 + }, + "left_arm": { + "elbow_roll": 0.0, + "elbow_yaw": 0.0, + "hand": 0.0, + "shoulder_pitch": 1.5707963705062866, + "shoulder_roll": 0.12999995052814484, + "wrist_yaw": -1.5707963705062866 + }, + "left_leg": { + "ankle_pitch": -0.524608314037323, + "ankle_roll": 5.960464477539064e-8, + "hip_pitch": -0.590949535369873, + "hip_roll": -1.1920928955078125e-7, + "hip_yaw_pitch": 7.902534626680335e-9, + "knee_pitch": 1.0650537014007568 + }, + "right_arm": { + "elbow_roll": 0.0, + "elbow_yaw": 0.0, + "hand": 0.0, + "shoulder_pitch": 1.5707963705062866, + "shoulder_roll": -0.12999995052814484, + "wrist_yaw": 1.5707963705062866 + }, + "right_leg": { + "ankle_pitch": -0.5242065787315369, + "ankle_roll": -5.960464477539064e-8, + "hip_pitch": -0.590949535369873, + "hip_roll": 0.00009464228060096502, + "hip_yaw_pitch": 7.902534626680335e-9, + "knee_pitch": 1.068637728691101 + } + }, + "motion": [ + { + "keyframes": [ + { + "duration": 0.1, + "positions": { + "head": { + "pitch": 0.34176361560821533, + "yaw": 0.6000000238418579 + }, + "left_arm": { + "elbow_roll": 0.0, + "elbow_yaw": -1.5, + "hand": 0.0, + "shoulder_pitch": 1.5707963705062866, + "shoulder_roll": 0.12999995052814484, + "wrist_yaw": -1.5707963705062866 + }, + "left_leg": { + "ankle_pitch": -0.524608314037323, + "ankle_roll": 5.960464477539064e-8, + "hip_pitch": -0.590949535369873, + "hip_roll": -1.1920928955078125e-7, + "hip_yaw_pitch": 7.902534626680335e-9, + "knee_pitch": 1.0650537014007568 + }, + "right_arm": { + "elbow_roll": 0.0, + "elbow_yaw": 1.5, + "hand": 0.0, + "shoulder_pitch": 1.5707963705062866, + "shoulder_roll": -0.12999995052814484, + "wrist_yaw": 1.5707963705062866 + }, + "right_leg": { + "ankle_pitch": -0.5242065787315369, + "ankle_roll": -5.960464477539064e-8, + "hip_pitch": -0.590949535369873, + "hip_roll": 0.00009464228060096502, + "hip_yaw_pitch": 7.902534626680335e-9, + "knee_pitch": 1.068637728691101 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.11194014549255372, + "yaw": 0.010695934295654295 + }, + "left_arm": { + "elbow_roll": -1.441918134689331, + "elbow_yaw": -1.5969362258911133, + "hand": 0.05359995365142822, + "shoulder_pitch": 1.5738420486450195, + "shoulder_roll": -0.1795198917388916, + "wrist_yaw": -1.0462298393249512 + }, + "left_leg": { + "ankle_pitch": -0.9741320610046388, + "ankle_roll": -0.009161949157714844, + "hip_pitch": -0.33437013626098633, + "hip_roll": 0.001575946807861328, + "hip_yaw_pitch": -0.4095361232757568, + "knee_pitch": 1.5891821384429932 + }, + "right_arm": { + "elbow_roll": 1.4880218505859375, + "elbow_yaw": 1.5354920625686646, + "hand": 0.06920003890991211, + "shoulder_pitch": 1.5938677787780762, + "shoulder_roll": 0.21165013313293457, + "wrist_yaw": 1.351412057876587 + }, + "right_leg": { + "ankle_pitch": -0.9832520484924316, + "ankle_roll": -0.01683211326599121, + "hip_pitch": -0.3206479549407959, + "hip_roll": -0.024502038955688477, + "hip_yaw_pitch": -0.4095361232757568, + "knee_pitch": 1.603072166442871 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.3543119430541992, + "yaw": 0.013764142990112305 + }, + "left_arm": { + "elbow_roll": 0.001575946807861328, + "elbow_yaw": 1.0216021537780762, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.5, + "shoulder_roll": -0.22093796730041504, + "wrist_yaw": -1.8070940971374512 + }, + "left_leg": { + "ankle_pitch": -0.754770040512085, + "ankle_roll": -0.018366098403930664, + "hip_pitch": -0.7961039543151855, + "hip_roll": 0.280822057723999, + "hip_yaw_pitch": -1.0752921104431152, + "knee_pitch": 2.162898063659668 + }, + "right_arm": { + "elbow_roll": 0.05986785888671875, + "elbow_yaw": -0.8099939823150635, + "hand": 0.06920003890991211, + "shoulder_pitch": 0.5, + "shoulder_roll": 0.21932005882263184, + "wrist_yaw": 0.5767421722412109 + }, + "right_leg": { + "ankle_pitch": -0.7378120422363281, + "ankle_roll": -0.001492023468017578, + "hip_pitch": -0.8360719680786133, + "hip_roll": -0.2808540439605713, + "hip_yaw_pitch": -1.0752921104431152, + "knee_pitch": 2.170651912689209 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.35584592819213867, + "yaw": 0.013764142990112305 + }, + "left_arm": { + "elbow_roll": -0.36044812202453613, + "elbow_yaw": 1.5308901071548462, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.47396397590637207, + "shoulder_roll": -0.13196587562561035, + "wrist_yaw": -0.7102839946746826 + }, + "left_leg": { + "ankle_pitch": 0.4449019432067871, + "ankle_roll": -0.41728997230529785, + "hip_pitch": -1.5478482246398926, + "hip_roll": 0.32056403160095215, + "hip_yaw_pitch": -1.2179540395736694, + "knee_pitch": 1.5769939422607422 + }, + "right_arm": { + "elbow_roll": 0.11969399452209473, + "elbow_yaw": -1.0017437934875488, + "hand": 0.06920003890991211, + "shoulder_pitch": 0.6872739791870117, + "shoulder_roll": 0.2469320297241211, + "wrist_yaw": 0.25460195541381836 + }, + "right_leg": { + "ankle_pitch": 0.4449019432067871, + "ankle_roll": 0.41728997230529785, + "hip_pitch": -1.5478482246398926, + "hip_roll": -0.32056403160095215, + "hip_yaw_pitch": -1.2179540395736694, + "knee_pitch": 1.5769939422607422 + } + } + }, + { + "duration": 2.0, + "positions": { + "head": { + "pitch": 0.35584592819213867, + "yaw": 0.013764142990112305 + }, + "left_arm": { + "elbow_roll": -0.36044812202453613, + "elbow_yaw": 1.5308901071548462, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.47396397590637207, + "shoulder_roll": -0.13196587562561035, + "wrist_yaw": -0.7102839946746826 + }, + "left_leg": { + "ankle_pitch": 0.4449019432067871, + "ankle_roll": -0.41728997230529785, + "hip_pitch": -1.5478482246398926, + "hip_roll": 0.32056403160095215, + "hip_yaw_pitch": -1.2179540395736694, + "knee_pitch": 1.5769939422607422 + }, + "right_arm": { + "elbow_roll": 0.11969399452209473, + "elbow_yaw": -1.0017437934875488, + "hand": 0.06920003890991211, + "shoulder_pitch": 0.6872739791870117, + "shoulder_roll": 0.2469320297241211, + "wrist_yaw": 0.25460195541381836 + }, + "right_leg": { + "ankle_pitch": 0.4449019432067871, + "ankle_roll": 0.41728997230529785, + "hip_pitch": -1.5478482246398926, + "hip_roll": -0.32056403160095215, + "hip_yaw_pitch": -1.2179540395736694, + "knee_pitch": 1.5769939422607422 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.570605993270874, + "yaw": 0.013764142990112305 + }, + "left_arm": { + "elbow_roll": -0.022968053817749023, + "elbow_yaw": -0.3436579704284668, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.3849921226501465, + "shoulder_roll": 0.07972598075866699, + "wrist_yaw": -1.331553936004639 + }, + "left_leg": { + "ankle_pitch": 1.135118007659912, + "ankle_roll": -0.052114009857177734, + "hip_pitch": -1.5109480619430542, + "hip_roll": 0.06753802299499512, + "hip_yaw_pitch": -1.14125394821167, + "knee_pitch": 0.7347440719604492 + }, + "right_arm": { + "elbow_roll": 0.033789873123168945, + "elbow_yaw": 0.5675380229949951, + "hand": 0.0700000524520874, + "shoulder_pitch": 0.4050178527832031, + "shoulder_roll": 0.10120201110839844, + "wrist_yaw": 1.8637681007385256 + }, + "right_leg": { + "ankle_pitch": 1.145939826965332, + "ankle_roll": 0.06753802299499512, + "hip_pitch": -1.526371955871582, + "hip_roll": -0.056715965270996094, + "hip_yaw_pitch": -1.14125394821167, + "knee_pitch": 0.7609059810638428 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.5675380229949951, + "yaw": 0.001492023468017578 + }, + "left_arm": { + "elbow_roll": -0.022968053817749023, + "elbow_yaw": -0.3436579704284668, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.3849921226501465, + "shoulder_roll": 0.07972598075866699, + "wrist_yaw": -1.331553936004639 + }, + "left_leg": { + "ankle_pitch": 1.0169999599456787, + "ankle_roll": -0.02143406867980957, + "hip_pitch": -1.2072160243988037, + "hip_roll": 0.25928807258605957, + "hip_yaw_pitch": -0.7454819679260254, + "knee_pitch": 0.3359041213989258 + }, + "right_arm": { + "elbow_roll": 0.033789873123168945, + "elbow_yaw": 0.5675380229949951, + "hand": 0.0700000524520874, + "shoulder_pitch": 0.4050178527832031, + "shoulder_roll": 0.10120201110839844, + "wrist_yaw": 1.8637681007385256 + }, + "right_leg": { + "ankle_pitch": 0.964928150177002, + "ankle_roll": -0.04444408416748047, + "hip_pitch": -1.3499622344970703, + "hip_roll": -0.08432793617248535, + "hip_yaw_pitch": -0.7454819679260254, + "knee_pitch": 0.5875639915466309 + } + } + }, + { + "duration": 0.3, + "positions": { + "head": { + "pitch": 0.5521979331970215, + "yaw": 0.007627964019775391 + }, + "left_arm": { + "elbow_roll": -0.022968053817749023, + "elbow_yaw": -0.3436579704284668, + "hand": 0.05359995365142822, + "shoulder_pitch": 0.3849921226501465, + "shoulder_roll": 0.07972598075866699, + "wrist_yaw": -1.331553936004639 + }, + "left_leg": { + "ankle_pitch": -0.6167099475860596, + "ankle_roll": 0.000041961669921875, + "hip_pitch": -1.5585020780563354, + "hip_roll": 0.000041961669921875, + "hip_yaw_pitch": 0.07213997840881348, + "knee_pitch": 0.6074221134185791 + }, + "right_arm": { + "elbow_roll": 0.033789873123168945, + "elbow_yaw": 0.5675380229949951, + "hand": 0.0700000524520874, + "shoulder_pitch": 0.4050178527832031, + "shoulder_roll": 0.10120201110839844, + "wrist_yaw": 1.8637681007385256 + }, + "right_leg": { + "ankle_pitch": -0.7654240131378174, + "ankle_roll": -0.010695934295654295, + "hip_pitch": -1.563188076019287, + "hip_roll": -0.01222991943359375, + "hip_yaw_pitch": 0.07213997840881348, + "knee_pitch": 0.6167099475860596 + } + } + }, + { + "duration": 0.4, + "positions": { + "head": { + "pitch": 0.5521979331970215, + "yaw": 0.007627964019775391 + }, + "left_arm": { + "elbow_roll": -1.286984086036682, + "elbow_yaw": -0.09668397903442384, + "hand": 0.05359995365142822, + "shoulder_pitch": 1.95427405834198, + "shoulder_roll": 0.9541060924530028, + "wrist_yaw": -1.526371955871582 + }, + "left_leg": { + "ankle_pitch": -0.7777800559997559, + "ankle_roll": 0.000041961669921875, + "hip_pitch": -1.553900122642517, + "hip_roll": -0.001492023468017578, + "hip_yaw_pitch": 0.07367396354675293, + "knee_pitch": 0.7853660583496094 + }, + "right_arm": { + "elbow_roll": 1.5601201057434082, + "elbow_yaw": 0.14108610153198242, + "hand": 0.0700000524520874, + "shoulder_pitch": 2.060204029083252, + "shoulder_roll": -1.3376898765563965, + "wrist_yaw": 1.537026047706604 + }, + "right_leg": { + "ankle_pitch": -0.7776961326599121, + "ankle_roll": -0.001492023468017578, + "hip_pitch": -1.549382209777832, + "hip_roll": -0.01222991943359375, + "hip_yaw_pitch": 0.07367396354675293, + "knee_pitch": 0.8069260120391846 + } + } + }, + { + "duration": 0.5, + "positions": { + "head": { + "pitch": 0.5521979331970215, + "yaw": 0.009161949157714844 + }, + "left_arm": { + "elbow_roll": -1.4158400297164917, + "elbow_yaw": 0.1748340129852295, + "hand": 0.05359995365142822, + "shoulder_pitch": 1.9910900592803955, + "shoulder_roll": 0.3573801517486572, + "wrist_yaw": -1.8623180389404297 + }, + "left_leg": { + "ankle_pitch": -0.7777800559997559, + "ankle_roll": 0.000041961669921875, + "hip_pitch": -1.553900122642517, + "hip_roll": -0.001492023468017578, + "hip_yaw_pitch": 0.07367396354675293, + "knee_pitch": 0.7853660583496094 + }, + "right_arm": { + "elbow_roll": 1.1766200065612793, + "elbow_yaw": -0.21633601188659668, + "hand": 0.0700000524520874, + "shoulder_pitch": 1.9942421913146973, + "shoulder_roll": -0.03225588798522949, + "wrist_yaw": 1.52168607711792 + }, + "right_leg": { + "ankle_pitch": -0.7776961326599121, + "ankle_roll": -0.001492023468017578, + "hip_pitch": -1.549382209777832, + "hip_roll": -0.01222991943359375, + "hip_yaw_pitch": 0.07367396354675293, + "knee_pitch": 0.8069260120391846 + } + } + } + ] + } + ] +} From 3e000d5ce7b75edd0e2d8971cdd56cc8d032430b Mon Sep 17 00:00:00 2001 From: larskna <143598820+larskna@users.noreply.github.com> Date: Mon, 15 Jul 2024 18:53:54 +0200 Subject: [PATCH 2/3] cleaned up --- crates/control/src/behavior/defend.rs | 4 ++-- crates/types/src/parameters.rs | 1 + etc/parameters/default.json | 3 ++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/crates/control/src/behavior/defend.rs b/crates/control/src/behavior/defend.rs index 1da264a81d..ec2e366137 100644 --- a/crates/control/src/behavior/defend.rs +++ b/crates/control/src/behavior/defend.rs @@ -75,9 +75,9 @@ impl<'cycle> Defend<'cycle> { match ( (-wide_stance_paramters.action_radius..=wide_stance_paramters.action_radius) .contains(&horizontal_distance_to_intersection), - (wide_stance_paramters.action_radius..0.6) + (wide_stance_paramters.action_radius..wide_stance_paramters.action_radius_left) .contains(&horizontal_distance_to_intersection), - (-0.6..-wide_stance_paramters.action_radius) + (-wide_stance_paramters.action_radius_left..-wide_stance_paramters.action_radius) .contains(&horizontal_distance_to_intersection), ) { (true, _, _) => Some(MotionCommand::WideStance { diff --git a/crates/types/src/parameters.rs b/crates/types/src/parameters.rs index 780589ced1..020e5688d9 100644 --- a/crates/types/src/parameters.rs +++ b/crates/types/src/parameters.rs @@ -389,6 +389,7 @@ pub struct SearchSuggestorParameters { pub struct WideStanceParameters { pub action_radius: f32, pub minimum_velocity: f32, + pub action_radius_left: f32, } #[derive( diff --git a/etc/parameters/default.json b/etc/parameters/default.json index 43e315518f..be1de9864f 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -563,7 +563,8 @@ }, "wide_stance": { "action_radius": 0.3, - "minimum_velocity": -0.5 + "minimum_velocity": -0.5, + "action_radius_left": 0.5 }, "kick_steps": { "forward": [ From 4b48ca6644e9d7fe0b92b3e3eaeefef38fc4f9e9 Mon Sep 17 00:00:00 2001 From: larskna <143598820+larskna@users.noreply.github.com> Date: Mon, 15 Jul 2024 19:05:19 +0200 Subject: [PATCH 3/3] fixed typo --- crates/control/src/behavior/defend.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/crates/control/src/behavior/defend.rs b/crates/control/src/behavior/defend.rs index ec2e366137..49b02a1683 100644 --- a/crates/control/src/behavior/defend.rs +++ b/crates/control/src/behavior/defend.rs @@ -58,14 +58,14 @@ impl<'cycle> Defend<'cycle> { pub fn wide_stance( &self, - wide_stance_paramters: WideStanceParameters, + wide_stance_parameters: WideStanceParameters, ) -> Option { let ball = self.world_state.ball?; let position = ball.ball_in_ground; let velocity = ball.ball_in_ground_velocity; - if velocity.x() >= wide_stance_paramters.minimum_velocity { + if velocity.x() >= wide_stance_parameters.minimum_velocity { return None; } @@ -73,11 +73,11 @@ impl<'cycle> Defend<'cycle> { position.y() - position.x() / velocity.x() * velocity.y(); match ( - (-wide_stance_paramters.action_radius..=wide_stance_paramters.action_radius) + (-wide_stance_parameters.action_radius..=wide_stance_parameters.action_radius) .contains(&horizontal_distance_to_intersection), - (wide_stance_paramters.action_radius..wide_stance_paramters.action_radius_left) + (wide_stance_parameters.action_radius..wide_stance_parameters.action_radius_left) .contains(&horizontal_distance_to_intersection), - (-wide_stance_paramters.action_radius_left..-wide_stance_paramters.action_radius) + (-wide_stance_parameters.action_radius_left..-wide_stance_parameters.action_radius) .contains(&horizontal_distance_to_intersection), ) { (true, _, _) => Some(MotionCommand::WideStance {