Behavior kick accuracy

crates/code_generation/src/cyclers.rs

@@ -58,7 +58,7 @@ fn generate_module(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -> Toke
     let cycler_implementation = generate_implementation(cycler, cyclers, mode);
 
     quote! {
-        #[allow(dead_code, unused_mut, unused_variables, clippy::too_many_arguments, clippy::needless_question_mark, clippy::borrow_deref_ref)]
+        #[allow(dead_code, unused_mut, unused_variables, clippy::too_many_arguments, clippy::needless_question_mark, clippy::borrow_deref_ref, clippy::explicit_auto_deref)]
         pub(crate) mod #module_name {
             use color_eyre::eyre::WrapErr;
             use crate::structs::#module_name::{MainOutputs, AdditionalOutputs};

crates/control/src/behavior/dribble.rs

@@ -2,13 +2,14 @@ use coordinate_systems::{Ground, UpcomingSupport};
 use geometry::look_at::LookAt;
 use linear_algebra::{Isometry2, Point, Pose2};
 use spl_network_messages::GamePhase;
+use std::time::{Duration, SystemTime};
 use types::{
     camera_position::CameraPosition,
     filtered_game_controller_state::FilteredGameControllerState,
     motion_command::{
         ArmMotion, HeadMotion, ImageRegion, MotionCommand, OrientationMode, WalkSpeed,
     },
-    parameters::{DribblingParameters, InWalkKickInfoParameters, InWalkKicksParameters},
+    parameters::{DribblingParameters, InWalkKicksParameters},
     planned_path::PathSegment,
     world_state::WorldState,
 };
@@ -23,6 +24,9 @@ pub fn execute(
     parameters: &DribblingParameters,
     dribble_path: Option<Vec<PathSegment>>,
     mut walk_speed: WalkSpeed,
+    precision_kick_timeout: Duration,
+    bigger_threshold_start_time: Option<SystemTime>,
+    cycle_start_time: SystemTime,
 ) -> Option<MotionCommand> {
     let ball_position = world_state.ball?.ball_in_ground;
     let distance_to_ball = ball_position.coords().norm();
@@ -39,14 +43,23 @@ pub fn execute(
     };
     let kick_decisions = world_state.kick_decisions.as_ref()?;
     let instant_kick_decisions = world_state.instant_kick_decisions.as_ref()?;
+    let abort_precision_kick = abort_precision_kick(
+        precision_kick_timeout,
+        bigger_threshold_start_time,
+        cycle_start_time,
+    );
 
     let available_kick = kick_decisions
         .iter()
         .chain(instant_kick_decisions.iter())
         .find(|decision| {
             is_kick_pose_reached(
                 decision.kick_pose,
-                &in_walk_kicks[decision.variant],
+                if !abort_precision_kick {
+                    in_walk_kicks[decision.variant].precision_kick_reached_thresholds
+                } else {
+                    in_walk_kicks[decision.variant].reached_thresholds
+                },
                 world_state.robot.ground_to_upcoming_support,
             )
         });
@@ -102,15 +115,31 @@ pub fn execute(
     }
 }
 
-fn is_kick_pose_reached(
+pub fn is_kick_pose_reached(
     kick_pose: Pose2<Ground>,
-    kick_info: &InWalkKickInfoParameters,
+    thresholds: nalgebra::Vector3<f32>,
     ground_to_upcoming_support: Isometry2<Ground, UpcomingSupport>,
 ) -> bool {
     let upcoming_kick_pose = ground_to_upcoming_support * kick_pose;
-    let is_x_reached = upcoming_kick_pose.position().x().abs() < kick_info.reached_thresholds.x;
-    let is_y_reached = upcoming_kick_pose.position().y().abs() < kick_info.reached_thresholds.y;
-    let is_orientation_reached =
-        upcoming_kick_pose.orientation().angle().abs() < kick_info.reached_thresholds.z;
+
+    let is_x_reached = upcoming_kick_pose.position().x().abs() < thresholds.x;
+    let is_y_reached = upcoming_kick_pose.position().y().abs() < thresholds.y;
+    let is_orientation_reached = upcoming_kick_pose.orientation().angle().abs() < thresholds.z;
+
     is_x_reached && is_y_reached && is_orientation_reached
 }
+
+pub fn abort_precision_kick(
+    precision_kick_timeout: Duration,
+    bigger_threshold_start_time: Option<SystemTime>,
+    cycle_start_time: SystemTime,
+) -> bool {
+    if bigger_threshold_start_time.is_some() {
+        let time_difference = cycle_start_time
+            .duration_since(bigger_threshold_start_time.unwrap())
+            .expect("Time ran back");
+        time_difference > precision_kick_timeout
+    } else {
+        false
+    }
+}

crates/control/src/behavior/mod.rs

@@ -1,7 +1,7 @@
 mod animation;
 mod calibrate;
 mod defend;
-mod dribble;
+pub mod dribble;
 mod fall_safely;
 mod head;
 mod initial;

crates/control/src/behavior/node.rs

@@ -1,4 +1,4 @@
-use std::time::SystemTime;
+use std::time::{Duration, SystemTime};
 
 use color_eyre::Result;
 use serde::{Deserialize, Serialize};
@@ -32,7 +32,8 @@ use crate::dribble_path_planner;
 use super::{
     animation, calibrate,
     defend::Defend,
-    dribble, fall_safely,
+    dribble::{self, is_kick_pose_reached},
+    fall_safely,
     head::LookAction,
     initial, intercept_ball, jump, look_around, lost_ball, no_ground_contact, penalize,
     prepare_jump, search, sit_down, stand, stand_up, support, unstiff, walk_to_kick_off,
@@ -46,6 +47,7 @@ pub struct Behavior {
     last_known_ball_position: Point2<Field>,
     active_since: Option<SystemTime>,
     previous_role: Role,
+    unprecise_threshold_start_time: Option<SystemTime>,
 }
 
 #[context]
@@ -62,7 +64,6 @@ pub struct CycleContext {
     world_state: Input<WorldState, "world_state">,
     cycle_time: Input<CycleTime, "cycle_time">,
     is_localization_converged: Input<bool, "is_localization_converged">,
-
     parameters: Parameter<BehaviorParameters, "behavior">,
     in_walk_kicks: Parameter<InWalkKicksParameters, "in_walk_kicks">,
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
@@ -82,6 +83,7 @@ pub struct CycleContext {
     support_walk_speed: Parameter<WalkSpeed, "walk_speed.support">,
     walk_to_kickoff_walk_speed: Parameter<WalkSpeed, "walk_speed.walk_to_kickoff">,
     walk_to_penalty_kick_walk_speed: Parameter<WalkSpeed, "walk_speed.walk_to_penalty_kick">,
+    precision_kick_timeout: Parameter<Duration, "precision_kick_timeout">,
 }
 
 #[context]
@@ -98,6 +100,7 @@ impl Behavior {
             last_known_ball_position: point![0.0, 0.0],
             active_since: None,
             previous_role: Role::Searcher,
+            unprecise_threshold_start_time: None,
         })
     }
 
@@ -275,6 +278,51 @@ impl Behavior {
             .dribble_path_obstacles_output
             .fill_if_subscribed(|| dribble_path_obstacles.clone().unwrap_or_default());
 
+        if let (Some(kick_decisions), Some(instant_kick_decisions)) = (
+            world_state.kick_decisions.as_ref(),
+            world_state.instant_kick_decisions.as_ref(),
+        ) {
+            let available_unprecise_kicks = kick_decisions
+                .iter()
+                .chain(instant_kick_decisions.iter())
+                .find(|decision| {
+                    is_kick_pose_reached(
+                        decision.kick_pose,
+                        context.in_walk_kicks[decision.variant].reached_thresholds,
+                        world_state.robot.ground_to_upcoming_support,
+                    )
+                });
+            let available_precise_kicks = kick_decisions
+                .iter()
+                .chain(instant_kick_decisions.iter())
+                .find(|decision| {
+                    is_kick_pose_reached(
+                        decision.kick_pose,
+                        context.in_walk_kicks[decision.variant].precision_kick_reached_thresholds,
+                        world_state.robot.ground_to_upcoming_support,
+                    )
+                });
+
+            match (
+                available_unprecise_kicks,
+                available_precise_kicks,
+                self.unprecise_threshold_start_time.is_none(),
+            ) {
+                (Some(available_unprecise_kicks), None, true) => {
+                    if is_kick_pose_reached(
+                        available_unprecise_kicks.kick_pose,
+                        context.in_walk_kicks[available_unprecise_kicks.variant].reached_thresholds,
+                        world_state.robot.ground_to_upcoming_support,
+                    ) {
+                        self.unprecise_threshold_start_time = Some(context.cycle_time.start_time);
+                    }
+                }
+                (_, Some(_), false) => {
+                    self.unprecise_threshold_start_time = Some(context.cycle_time.start_time);
+                }
+                _ => {}
+            }
+        }
         let (action, motion_command) = actions
             .iter()
             .find_map(|action| {
@@ -376,6 +424,9 @@ impl Behavior {
                         &context.parameters.dribbling,
                         dribble_path.clone(),
                         *context.dribble_walk_speed,
+                        *context.precision_kick_timeout,
+                        self.unprecise_threshold_start_time,
+                        context.cycle_time.start_time,
                     ),
                     Action::Jump => jump::execute(world_state),
                     Action::PrepareJump => prepare_jump::execute(world_state),

crates/control/src/kick_selector.rs

@@ -108,6 +108,12 @@ impl KickSelector {
             context.in_walk_kicks,
         );
 
+        kick_decisions.retain(|target| match target.variant {
+            KickVariant::Forward => context.in_walk_kicks.forward.enabled,
+            KickVariant::Turn => context.in_walk_kicks.turn.enabled,
+            KickVariant::Side => context.in_walk_kicks.side.enabled,
+        });
+
         kick_decisions.sort_by(|left, right| {
             compare_decisions(
                 left,
@@ -359,6 +365,7 @@ fn generate_decisions_for_instant_kicks(
             struct TargetAlignedBall;
             struct KickPose;
             let position: Point2<TargetAlignedBall> = Point2::wrap(kick_info.position);
+
             let parameter_kick_pose =
                 Pose2::from_parts(position, Orientation2::new(kick_info.orientation));
             let target_aligned_ball_to_kick_pose = match kicking_side {

crates/types/src/parameters.rs

@@ -129,6 +129,7 @@ pub struct InWalkKickInfoParameters {
     pub position_offset: nalgebra::Vector2<f32>,
     pub orientation: f32,
     pub reached_thresholds: nalgebra::Vector3<f32>,
+    pub precision_kick_reached_thresholds: nalgebra::Vector3<f32>,
     pub shot_distance: f32,
     pub enabled: bool,
 }

etc/parameters/default.json

@@ -7,7 +7,7 @@
     "minimum_overall_keypoint_confidence": 0.5,
     "minimum_visual_referee_keypoint_confidence": 0.8,
     "minimum_shoulder_angle": 0.2,
-    "foot_z_offset": 0.025,
+    "foot_z_offset": 0.1,
     "referee_pose_queue_length": 8,
     "minimum_number_poses_before_message": 3,
     "override_pose_detection": false
@@ -1071,27 +1071,31 @@
       "position": [-0.22, -0.04],
       "position_offset": [0.0, 0.0],
       "orientation": 0.0,
-      "reached_thresholds": [0.04, 0.02, 0.05],
+      "reached_thresholds": [0.06, 0.04, 0.07],
+      "precision_kick_reached_thresholds": [0.04, 0.02, 0.05],
       "shot_distance": 4.0,
       "enabled": true
     },
     "turn": {
-      "position": [-0.15, -0.11],
-      "position_offset": [0.0, 0.0],
+      "position": [-0.145, -0.08],
       "orientation": 0.9,
-      "reached_thresholds": [0.04, 0.04, 0.1],
+      "position_offset": [0.0, 0.0],
+      "reached_thresholds": [0.06, 0.06, 0.12],
+      "precision_kick_reached_thresholds": [0.04, 0.04, 0.1],
       "shot_distance": 3.5,
       "enabled": true
     },
     "side": {
       "position": [-0.15, 0.03],
       "position_offset": [0.0, 0.0],
       "orientation": -1.57,
-      "reached_thresholds": [0.05, 0.06, 0.1],
+      "reached_thresholds": [0.07, 0.08, 0.12],
+      "precision_kick_reached_thresholds": [0.05, 0.06, 0.1],
       "shot_distance": 0.8,
       "enabled": true
     }
   },
+  "precision_kick_timeout": { "nanos": 0, "secs": 2},
   "kick_selector": {
     "distance_to_corner": 1.5,
     "corner_kick_target_distance_to_goal": 1.3,