Mpc step planning

Cargo.toml

@@ -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"

crates/bevyhavior_simulator/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/src/autoref.rs

@@ -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/bin/intercept_ball.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/kick_in.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/mpc_step_planning_optimizer.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/step_planning_test.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/control/Cargo.toml

@@ -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/src/behavior/defend.rs

@@ -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,
         )

crates/control/src/behavior/dribble.rs

@@ -12,14 +12,14 @@ use types::{
 
 use super::walk_to_pose::WalkPathPlanner;
 
-#[allow(clippy::too_many_arguments)]
 pub fn execute(
     world_state: &WorldState,
     walk_path_planner: &WalkPathPlanner,
     in_walk_kicks: &InWalkKicksParameters,
     parameters: &DribblingParameters,
     dribble_path_plan: Option<DribblePathPlan>,
     mut walk_speed: WalkSpeed,
+    distance_to_be_aligned: f32,
 ) -> Option<MotionCommand> {
     let ball_position = world_state.ball?.ball_in_ground;
     let distance_to_ball = ball_position.coords().norm();
@@ -70,10 +70,13 @@ pub fn execute(
     match dribble_path_plan {
         Some(DribblePathPlan {
             orientation_mode,
+            target_orientation,
             path,
         }) => Some(walk_path_planner.walk_with_obstacle_avoiding_arms(
             head,
             orientation_mode,
+            target_orientation,
+            distance_to_be_aligned,
             path,
             walk_speed,
         )),

crates/control/src/behavior/intercept_ball.rs

@@ -1,7 +1,7 @@
 use coordinate_systems::{Field, Ground};
-use geometry::line::Line;
 use geometry::line_segment::LineSegment;
-use linear_algebra::{Isometry2, Orientation2, Point};
+use geometry::{line::Line, look_at::LookAt};
+use linear_algebra::{Isometry2, Point};
 use spl_network_messages::{GamePhase, SubState};
 use types::{
     filtered_game_controller_state::FilteredGameControllerState,
@@ -16,6 +16,7 @@ pub fn execute(
     world_state: &WorldState,
     parameters: InterceptBallParameters,
     walk_speed: WalkSpeed,
+    distance_to_be_aligned: f32,
 ) -> Option<MotionCommand> {
     if let Some(
         FilteredGameControllerState {
@@ -69,6 +70,7 @@ pub fn execute(
                 ))],
             };
 
+            let target_orientation = interception_point.look_at(&ball.ball_in_ground);
             Some(MotionCommand::Walk {
                 head: HeadMotion::LookAt {
                     target: ball.ball_in_ground,
@@ -78,8 +80,13 @@ pub fn execute(
                 path,
                 left_arm: types::motion_command::ArmMotion::Swing,
                 right_arm: types::motion_command::ArmMotion::Swing,
-                orientation_mode: OrientationMode::Override(Orientation2::identity()),
+                orientation_mode: OrientationMode::LookTowards {
+                    direction: target_orientation,
+                    tolerance: 0.0,
+                },
                 speed: walk_speed,
+                target_orientation,
+                distance_to_be_aligned,
             })
         }
         _ => None,

crates/control/src/behavior/look_at_referee.rs

@@ -3,7 +3,7 @@ use framework::AdditionalOutput;
 use linear_algebra::{Point2, Pose2, Rotation2};
 use types::{
     camera_position::CameraPosition,
-    motion_command::{HeadMotion, ImageRegion, MotionCommand, WalkSpeed},
+    motion_command::{HeadMotion, ImageRegion, MotionCommand, OrientationMode, WalkSpeed},
     path_obstacles::PathObstacle,
     world_state::WorldState,
 };
@@ -42,6 +42,7 @@ pub fn execute(
         head_motion,
         path_obstacles_output,
         walk_speed,
+        OrientationMode::AlignWithPath,
         distance_to_be_aligned,
         walk_and_stand.parameters.hysteresis,
     )