Mpc step planning (#1854)

* mpc step planning

* taplo fmt

* Nuke `traits::LossField`

* Use parameters instead of hard-coded constants

* Move test around

* Organize imports

* Add todo comment

* undo changes in golden goal

* Rename `AlignWithPath` -> `Unspecified` to reflect new semantics

* Refactor: remove `StepPlanningLossField`

* Clean up old comments

* Fix failing test

* Borrow `Path`

* Borrow `Parameters`

* Naming consistency

* Remove unused variable

* Remove another unused variable

* Add todo comment

* Remove outdated comment

* Re-implement customizable walk volume exponents

* Remove unused parameters

* Fix parameter path

* Undo accidental change

* Remove todo comment

* Remove outdated test comment

* Update todo comment

* Reduce backwards step length

* Verify path_distance gradient

* Add some new gradient types

* Pose: use `Angle` for orientation

* Add more gradient verification tests

* Use test_path fn from test_utils module

* Refactor module layout

* Update behavior sim scenarios

* Fix bugs in target orientation field

* Make learning rate configurable

* Expose current support side and draw soles in twix

* Expose step plan cost

* Also calculate greedy step plan

* Remove pub use

* Use correct norm for normalizing gradient

* Fix greedy shit

* Remove `PoseAndSupportFoot` struct

* Rename variable

* Simulate support foot switch in greedy step plan

* Remove `dbg`

* Fix incorrect fake gradient size

* --wip--

* Change mpc test scenario to use fixed path

* --wip--

* Use OpEn/PANOC solver (WIP!)

* Fix step drawing

* Improve step plan drawing

* Improve mpc demo scenario path

* Normalize gradient

* Tweak parameters

* Tweak demo scenario path

* Fix incorrect step plan drawing (again)

* Remove unused parameter

* Remove transparency of planned steps

* Paint 0th step

* Update optimization parameters

* Remove `PlannedStep`

* Remove unused parameters

* Add benchmark

* Optimization: precompute cost function parts

* Remove unused field

* Remove unused field

* Tiny refactor

* Add `EndPoints` trait

* Refactor: reuse length impl

* Tiny refactor

* impl endpoints for path

* Take direct step to target if possible

* Fix occasional instability

* Organize step planning cost factors

* Don't panic if solver fails

* Fix trailing comma

* Remove unused import

* Refactor alignment importance scaling

* Verify walk orientation gradient

* Remove hybrid alignment

* Recycle variables of previous cycle

* Forget previous step plan on support side switch

* Update step planning test scenario

* Enable walk and target orientation

* Tweak parameters to fix donking

* Add warm start parameter

* Refactor

* Greedy step plan: Clamp to anatomical constraints

* Fix missing struct field

* Twix: draw target orientation

* Refactor

* Re-add `OrientationMode::AlignWithPath`

* Change some magic numbers

* Move `WalkVolumeExtents` out of `StepPlanningOptimizationParameters`

* Use constant for number of variables per step

* Move `gradient_type` module to its own file

* Rework step planner parameters and step clamping

* Remove duplicate gradient_type module

* Add `PoseGradient::zeros()` convenience method

* Remove unneccessary check

* Fix sign flip in path alignment

* Add proptest for `OrientationMode::AlignWithPath`

* Use `OrientationMode::AlignWithPath` in `walk_to_pose`

* Remove unneccessary `abs()`

* Reduce path alignment tolerance parameter

* Add tolerance field to `OrientationMode`

* Add orientation mode parameter to `walk_to_pose`

and force kickoff striker to look towards the other field half

* Fix searching behavior

* Tweak target orientation in lost_ball

* Add todo comment

* Move `StepPlanning` out of `step_plan.rs`

* Re-add PR #1914, which was removed in a rebase

this was easier than correctly rebasing.

* Use `Path` newtype

* Fix another timing issue

* Move step planning proptest function bodies out of macro

* Reject proptest samples on test path discontinuities

* Add proptest config to increase number of samples

* Add parameter to disable MPC step planning

* Constify number of steps

* Remove todo

* Remove `max_step_size` parameter

* Add `Orientation` type

* Consolidate `WrapGradient` and `UnwrapGradient`

* Move test_path out of cfg(test) gate

Otherwise, it could not be used across crate boundaries and in external benches/tests.

* penalize orientation towards the wrong side of the path

* Pin OpEn dependency

* Remove step_planning_duration

* Clamp inside movement without anatomical constraints

* Rename `plan_step` to make intent clearer

* Adress review comments

* Remove unnecessary special case for striker walk-in

* Remove unused utilities

* Move test_utils module to its own file

* Organize imports

* Refactor: use hybrid alignment distance again

* Re-implement hybrid alignment for greedy step plan

* Refactor direct step and fix a few issues

Before, direct step was not working for the greedy step planner

* Remove leftover comment

* Naming consistency

* Use methods of `Path` type

---------

Co-authored-by: Konrad Nölle <konrad@familie-noelle.de>

Author: rmburg

Cargo.lock

@@ -43,6 +43,8 @@ members = [
   "crates/source_analyzer",
   "crates/spl_network",
   "crates/spl_network_messages",
+  "crates/step_planning",
+  "crates/step_planning_solver",
   "crates/types",
   "crates/vision",
   "crates/walking_engine",
@@ -164,11 +166,13 @@ ndarray-conv = "0.4.1"
 nix = { version = "0.29", features = ["ioctl"] }
 nucleo-matcher = "0.3.1"
 num-derive = "0.4.2"
+num-dual = "0.11.1"
 num-traits = "0.2"
 object_detection = { path = "crates/object_detection" }
 once_cell = "1.20.3"
 openvino = { version = "0.8.0", features = ["runtime-linking"] }
 opn = { path = "crates/opn" }
+optimization_engine = "0.9.1"
 opusfile-ng = "0.1.0"
 ordered-float = "4.6.0"
 parameters = { path = "crates/parameters" }
@@ -214,6 +218,8 @@ source_analyzer = { path = "crates/source_analyzer" }
 spl_network = { path = "crates/spl_network" }
 spl_network_messages = { path = "crates/spl_network_messages" }
 splines = { version = "=4.2.0", features = ["serde"] }
+step_planning = { path = "crates/step_planning" }
+step_planning_solver = { path = "crates/step_planning_solver" }
 structopt = "0.3.26"
 syn = { version = "2.0.98", features = ["extra-traits", "full"] }
 systemd = "0.10.0"

Cargo.toml

@@ -42,6 +42,7 @@ serde = { workspace = true }
 serde_json = { workspace = true }
 spl_network = { workspace = true }
 spl_network_messages = { workspace = true }
+step_planning = { workspace = true }
 tokio = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }

crates/bevyhavior_simulator/Cargo.toml

@@ -13,11 +13,11 @@ use bevy::{
 use coordinate_systems::{Field, Ground};
 use linear_algebra::{point, vector, Isometry2};
 use spl_network_messages::{GameState, Penalty, SubState, Team};
+use step_planning::traits::Length;
 use types::{
     ball_position::SimulatorBallState,
     field_dimensions::{FieldDimensions, Half, Side},
     motion_command::MotionCommand,
-    planned_path::PathSegment,
 };
 
 use crate::{
@@ -71,11 +71,7 @@ pub fn autoref(
                     MotionCommand::Unstiff
                     | MotionCommand::Penalized
                     | MotionCommand::Stand { .. } => false,
-                    MotionCommand::Walk { path, .. }
-                        if path.segments.iter().map(PathSegment::length).sum::<f32>() < 0.01 =>
-                    {
-                        false
-                    }
+                    MotionCommand::Walk { path, .. } if path.length() < 0.01 => false,
                     _ => true,
                 }
             });

crates/bevyhavior_simulator/src/autoref.rs

@@ -33,8 +33,8 @@ fn startup(
 ) {
     let mut robot = Robot::new(PlayerNumber::One);
     *robot.ground_to_field_mut() = Isometry2::from_parts(vector![-2.0, 0.0], 0.0);
-    robot.parameters.step_planner.max_step_size.forward = 1.0;
-    robot.parameters.step_planner.max_step_size.left = 1.0;
+    robot.parameters.step_planner.walk_volume_extents.forward = 1.0;
+    robot.parameters.step_planner.walk_volume_extents.outward = 1.0;
     robot.parameters.step_planner.request_scale = Step {
         forward: 1.0,
         left: 1.0,

crates/bevyhavior_simulator/src/bin/intercept_ball.rs

@@ -42,14 +42,14 @@ fn update(
     robots: Query<&mut Robot>,
     mut exit: EventWriter<AppExit>,
 ) {
-    if time.ticks() == 6000 {
+    if time.ticks() == 5000 {
         game_controller_commands.send(GameControllerCommand::SetSubState(
             Some(SubState::KickIn),
             Team::Hulks,
             None,
         ));
     }
-    if time.ticks() >= 6005 && time.ticks() < 6000 + FREE_KICK_DURATION_IN_TICKS {
+    if time.ticks() >= 5005 && time.ticks() < 5000 + FREE_KICK_DURATION_IN_TICKS {
         for robot in robots.iter() {
             if robot
                 .database

crates/bevyhavior_simulator/src/bin/kick_in.rs

@@ -0,0 +1,82 @@
+use std::f32::consts::FRAC_PI_2;
+
+use bevy::prelude::*;
+
+use bevyhavior_simulator::{
+    game_controller::{GameController, GameControllerCommand},
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
+use linear_algebra::{point, vector, Isometry2, Orientation2, Point2};
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber};
+use types::{
+    motion_command::{ArmMotion, HeadMotion, MotionCommand, OrientationMode, WalkSpeed},
+    planned_path::{Path, PathSegment},
+};
+
+#[scenario]
+fn mpc_step_planner_optimizer(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+fn startup(
+    mut commands: Commands,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    commands.spawn(Robot::new(PlayerNumber::Seven));
+
+    game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Playing));
+}
+
+fn update(
+    _game_controller: ResMut<GameController>,
+    time: Res<Time<Ticks>>,
+    mut exit: EventWriter<AppExit>,
+    mut robots: Query<&mut Robot>,
+) {
+    let mut robot = robots.single_mut();
+
+    robot.database.main_outputs.ground_to_field =
+        Some(Isometry2::from_parts(vector![-1.0, -1.0], FRAC_PI_2));
+    robot.parameters.behavior.injected_motion_command = Some(MotionCommand::Walk {
+        head: HeadMotion::ZeroAngles,
+        left_arm: ArmMotion::Swing,
+        right_arm: ArmMotion::Swing,
+        speed: WalkSpeed::Normal,
+        path: Path {
+            segments: vec![
+                PathSegment::LineSegment(LineSegment(Point2::origin(), point![0.3, 0.0])),
+                PathSegment::Arc(Arc {
+                    circle: Circle {
+                        center: point![0.3, 0.3],
+                        radius: 0.3,
+                    },
+                    start: Orientation2::new(3.0 * FRAC_PI_2),
+                    end: Orientation2::new(0.0),
+                    direction: Direction::Counterclockwise,
+                }),
+                PathSegment::LineSegment(LineSegment(point![0.6, 0.3], point![0.6, 0.8])),
+            ],
+        },
+        orientation_mode: OrientationMode::Unspecified,
+        target_orientation: Orientation2::identity(),
+        distance_to_be_aligned: 0.1,
+    });
+
+    let optimizer_steps = time.ticks() as usize;
+    robots
+        .single_mut()
+        .parameters
+        .step_planner
+        .optimization_parameters
+        .optimizer_steps = optimizer_steps;
+
+    println!("tick {}: {optimizer_steps} steps", time.ticks());
+
+    if time.ticks() >= 500 {
+        exit.send(AppExit::Success);
+    }
+}

crates/bevyhavior_simulator/src/bin/mpc_step_planning_optimizer.rs

@@ -0,0 +1,101 @@
+use std::f32::consts::TAU;
+
+use bevy::prelude::*;
+
+use bevyhavior_simulator::{
+    game_controller::GameControllerCommand,
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use linear_algebra::{point, Isometry2, Orientation2, Point2};
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber};
+use types::{
+    motion_command::{ArmMotion, HeadMotion, MotionCommand, OrientationMode, WalkSpeed},
+    planned_path::{Path, PathSegment},
+};
+
+#[scenario]
+fn demonstration(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+fn startup(
+    mut commands: Commands,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    commands.spawn(Robot::new(PlayerNumber::Seven));
+    game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Playing));
+}
+
+fn update(time: Res<Time<Ticks>>, mut exit: EventWriter<AppExit>, mut robots: Query<&mut Robot>) {
+    let mut robot = robots.iter_mut().next().unwrap();
+
+    robot
+        .parameters
+        .step_planner
+        .optimization_parameters
+        .optimizer_steps = 100;
+    robot
+        .parameters
+        .step_planner
+        .optimization_parameters
+        .warm_start = false;
+    robot.database.main_outputs.ground_to_field = Some(Isometry2::identity());
+
+    let angle = 0.01 * time.ticks() as f32;
+    let (sin, cos) = angle.sin_cos();
+    let turn_count = (angle / TAU) as usize;
+
+    let orbiting_point = point![cos, sin] * 0.4;
+    let orientation = Orientation2::from_cos_sin_unchecked(cos, sin);
+
+    let (path, orientation_mode, target_orientation) = match turn_count {
+        0 => (
+            Path {
+                segments: vec![PathSegment::LineSegment(
+                    geometry::line_segment::LineSegment(Point2::origin(), orbiting_point),
+                )],
+            },
+            OrientationMode::Unspecified,
+            Orientation2::identity(),
+        ),
+        1 => (
+            Path {
+                segments: vec![PathSegment::LineSegment(
+                    geometry::line_segment::LineSegment(Point2::origin(), orbiting_point),
+                )],
+            },
+            OrientationMode::Unspecified,
+            orientation,
+        ),
+        2 => (
+            Path {
+                segments: vec![PathSegment::LineSegment(
+                    geometry::line_segment::LineSegment(Point2::origin(), point![0.4, 0.0]),
+                )],
+            },
+            OrientationMode::LookTowards {
+                direction: orientation,
+                tolerance: 0.0,
+            },
+            orientation,
+        ),
+        _ => {
+            exit.send(AppExit::Success);
+            return;
+        }
+    };
+
+    robot.parameters.behavior.injected_motion_command = Some(MotionCommand::Walk {
+        path,
+        orientation_mode,
+        target_orientation,
+        distance_to_be_aligned: 0.1,
+        head: HeadMotion::ZeroAngles,
+        left_arm: ArmMotion::Swing,
+        right_arm: ArmMotion::Swing,
+        speed: WalkSpeed::Normal,
+    });
+}

crates/bevyhavior_simulator/src/bin/step_planning_test.rs

@@ -38,5 +38,7 @@ serde = { workspace = true }
 smallvec = { workspace = true }
 spl_network_messages = { workspace = true }
 splines = { workspace = true }
+step_planning = { workspace = true }
+step_planning_solver = { workspace = true }
 types = { workspace = true }
 walking_engine = { workspace = true }

crates/control/Cargo.toml

@@ -13,7 +13,7 @@ use spl_network_messages::{GamePhase, SubState, Team};
 use types::{
     field_dimensions::{FieldDimensions, Side},
     filtered_game_controller_state::FilteredGameControllerState,
-    motion_command::{JumpDirection, MotionCommand, WalkSpeed},
+    motion_command::{JumpDirection, MotionCommand, OrientationMode, WalkSpeed},
     parameters::{KeeperMotionParameters, RolePositionsParameters},
     path_obstacles::PathObstacle,
     world_state::{BallState, WorldState},
@@ -68,6 +68,8 @@ impl<'cycle> Defend<'cycle> {
             self.look_action.execute(),
             path_obstacles_output,
             walk_speed,
+            // TODO(rmburg): maybe change this instead of having a large distance_to_be_aligned?
+            OrientationMode::AlignWithPath,
             distance_to_be_aligned,
             hysteresis,
         )