Tuned parameters from match versus RMIT (#643)

Co-authored-by: Harold Ruiter <harold@soaratorium.com>

Author: oxkitsune

deploy/config/ball_detection.toml

@@ -6,10 +6,10 @@ max_classification_age_eye_color = 100
 ball_ratio = 0.35
 
 # The ratio of green pixels in the patch below the ball.
-grass_ratio = 0.70
+grass_ratio = 0.7
 
 # Height/width of the bounding box around the ball
-bounding_box_scale = 95.0
+bounding_box_scale = 80.0
 
 
 # The minimum overlap ratio between for bounding boxes to be merged using non-maximum suppression
@@ -39,10 +39,10 @@ bbox_padding_factor = 1.1
 ball_ratio = 0.35
 
 # The ratio of green pixels in the patch below the ball.
-grass_ratio = 0.70
+grass_ratio = 0.1
 
 # Height/width of the bounding box around the ball
-bounding_box_scale = 28.5
+bounding_box_scale = 30.0
 
 # The minimum overlap ratio between for bounding boxes to be merged using non-maximum suppression
 nms_threshold = 0.3
@@ -64,7 +64,7 @@ grass_patch_y_offset_factor = 0.5
 grass_patch_radius_factor = 1.5
 
 # Padding factor applied to final proposal box sizes to account for camera calibration error.
-bbox_padding_factor = 1.4
+bbox_padding_factor = 1.1
 
 [classifier]
 # The threshold for the classifier to consider a proposal a ball

deploy/config/behavior.toml

@@ -11,7 +11,7 @@ head_pitch_max = 0.25
 [rl_striker_search]
 # The output of the policy is element wise multiplied with this value to determine the
 # step that is requested to the walking engine.
-policy_output_scaling = { forward = 0.05, left = 0.07, turn = 0.2 }
+policy_output_scaling = { forward = 0.055, left = 0.07, turn = 0.2 }
 # Controls how fast the robot moves its head back and forth while looking around
 head_rotation_speed = 3.0
 # Controls how far to the left and right the robot looks while looking around, in radians.

deploy/config/walking_engine.toml

@@ -1,5 +1,5 @@
 # The base amount of time (in milliseconds) for one step.
-base_step_duration = 225
+base_step_duration = 240
 
 # Duration modifiers (in seconds) for a single step.
 # Modifies `base_step_duration` by adding (modifier * movement factor) seconds per step.
@@ -16,7 +16,7 @@ stopping_step_duration = 250
 # This value is normalized relative to the planned step duration. For example,
 # a value of `0.75` means the current step duration must be at least 75% of the
 # planned step duration before a foot switch can occur.
-minimum_step_duration_ratio = 0.75
+minimum_step_duration_ratio = 0.70
 
 # The offset of the torso w.r.t. the hips in meters.
 #
@@ -48,7 +48,7 @@ arm_stiffness = 0.6
 
 # The base amount to lift the feet in swing phase, in metres.
 # The foot lift is increased slightly, based on the forward and left in the command.
-base_foot_lift = 0.005
+base_foot_lift = 0.007
 
 # The multiplier for each component of a step command to adjust the foot lift.
 # These values are multiplied by their respective component of the step command
@@ -68,7 +68,7 @@ stopping_foot_lift = 0.01
 max_acceleration = { forward = 0.01, left = 0.05, turn = 0.3 }
 
 # The step size is clipped to this value; in both directions (e.g. range for forward is -max_step_size to max_step_size).
-max_step_size = { forward = 0.055, left = 0.05, turn = 0.7 }
+max_step_size = { forward = 0.06, left = 0.06, turn = 0.7 }
 
 
 [hip_height]
@@ -99,7 +99,7 @@ arm_swing_multiplier = 0.6
 #
 # Higher values mean that the filtered gyroscope value responds to changes quicker,
 # and lower values mean that it responds slower.
-gyro_lpf_alpha = 0.6
+gyro_lpf_alpha = 0.7
 
 # The weight of the balance adjustment based on the y gyroscope value.
 # Increasing this value will use a larger portion of the filtered gyro value

yggdrasil/src/motion/step_planner.rs

@@ -8,8 +8,10 @@ use nalgebra::{Isometry, Point2, UnitComplex, Vector2};
 use rerun::{FillMode, LineStrip3D};
 use std::time::Instant;
 
-const TURN_SPEED: f32 = 0.2;
-const WALK_SPEED: f32 = 0.045;
+const TURN_SPEED: f32 = 0.4;
+const MIN_TURN_MULTIPLIER: f32 = 0.5;
+const MAX_TURN_MULTIPLIER: f32 = 1.0;
+const WALK_SPEED: f32 = 0.050;
 
 const PRECISE_WALK_DISTANCE: f32 = 0.2;
 
@@ -169,10 +171,10 @@ impl StepPlanner {
     }
 
     fn plan_rotation(robot_pose: &RobotPose, target_rotation: UnitComplex<f32>) -> Option<Step> {
-        let angle = target_rotation.angle() - robot_pose.world_rotation();
-        let turn = TURN_SPEED * angle.signum();
+        let angle_err = target_rotation.angle() - robot_pose.world_rotation();
+        let turn = scale_turn_speed(angle_err);
 
-        if angle.abs() < 0.2 {
+        if angle_err.abs() < 0.2 {
             None
         } else {
             Some(Step {
@@ -380,3 +382,26 @@ fn log_dynamic_obstacles(dbg: DebugContext, step_planner: Res<StepPlanner>, cycl
             .with_half_sizes(half_sizes),
     );
 }
+
+#[inline(always)]
+fn scale_turn_speed(yaw_err: f32) -> f32 {
+    use std::f32::consts::PI;
+
+    // Wrap to (-PI, PI]
+    let mut e = yaw_err;
+    if e > PI || e <= -PI {
+        e = (e + PI).rem_euclid(2.0 * PI) - PI;
+    }
+
+    let mag = e.abs();
+
+    if mag < 1e-4 {
+        return 0.0;
+    }
+
+    // Linear ramp: MIN_TURN_SPEED .. TURN_SPEED over 0..FULL_SPEED_ANGLE
+    let frac = (mag / MAX_TURN_MULTIPLIER).min(1.0);
+    let spd = MIN_TURN_MULTIPLIER + frac * (TURN_SPEED - MIN_TURN_MULTIPLIER);
+
+    spd.copysign(e)
+}