|
| 1 | +use std::f32::consts::FRAC_PI_2; |
| 2 | + |
| 3 | +use bevy::prelude::*; |
| 4 | + |
| 5 | +use bevyhavior_simulator::{ |
| 6 | + game_controller::{GameController, GameControllerCommand}, |
| 7 | + robot::Robot, |
| 8 | + time::{Ticks, TicksTime}, |
| 9 | +}; |
| 10 | +use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment}; |
| 11 | +use linear_algebra::{point, vector, Isometry2, Orientation2, Point2}; |
| 12 | +use scenario::scenario; |
| 13 | +use spl_network_messages::{GameState, PlayerNumber}; |
| 14 | +use types::{ |
| 15 | + motion_command::{ArmMotion, HeadMotion, MotionCommand, OrientationMode, WalkSpeed}, |
| 16 | + planned_path::{Path, PathSegment}, |
| 17 | +}; |
| 18 | + |
| 19 | +#[scenario] |
| 20 | +fn mpc_step_planner_optimizer(app: &mut App) { |
| 21 | + app.add_systems(Startup, startup); |
| 22 | + app.add_systems(Update, update); |
| 23 | +} |
| 24 | + |
| 25 | +fn startup( |
| 26 | + mut commands: Commands, |
| 27 | + mut game_controller_commands: EventWriter<GameControllerCommand>, |
| 28 | +) { |
| 29 | + commands.spawn(Robot::new(PlayerNumber::Seven)); |
| 30 | + |
| 31 | + game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Playing)); |
| 32 | +} |
| 33 | + |
| 34 | +fn update( |
| 35 | + _game_controller: ResMut<GameController>, |
| 36 | + time: Res<Time<Ticks>>, |
| 37 | + mut exit: EventWriter<AppExit>, |
| 38 | + mut robots: Query<&mut Robot>, |
| 39 | +) { |
| 40 | + let mut robot = robots.single_mut(); |
| 41 | + |
| 42 | + robot.database.main_outputs.ground_to_field = |
| 43 | + Some(Isometry2::from_parts(vector![-1.0, -1.0], FRAC_PI_2)); |
| 44 | + robot.parameters.behavior.injected_motion_command = Some(MotionCommand::Walk { |
| 45 | + head: HeadMotion::ZeroAngles, |
| 46 | + left_arm: ArmMotion::Swing, |
| 47 | + right_arm: ArmMotion::Swing, |
| 48 | + speed: WalkSpeed::Normal, |
| 49 | + path: Path { |
| 50 | + segments: vec![ |
| 51 | + PathSegment::LineSegment(LineSegment(Point2::origin(), point![0.3, 0.0])), |
| 52 | + PathSegment::Arc(Arc { |
| 53 | + circle: Circle { |
| 54 | + center: point![0.3, 0.3], |
| 55 | + radius: 0.3, |
| 56 | + }, |
| 57 | + start: Orientation2::new(3.0 * FRAC_PI_2), |
| 58 | + end: Orientation2::new(0.0), |
| 59 | + direction: Direction::Counterclockwise, |
| 60 | + }), |
| 61 | + PathSegment::LineSegment(LineSegment(point![0.6, 0.3], point![0.6, 0.8])), |
| 62 | + ], |
| 63 | + }, |
| 64 | + orientation_mode: OrientationMode::Unspecified, |
| 65 | + target_orientation: Orientation2::identity(), |
| 66 | + distance_to_be_aligned: 0.1, |
| 67 | + }); |
| 68 | + |
| 69 | + let optimizer_steps = time.ticks() as usize; |
| 70 | + robots |
| 71 | + .single_mut() |
| 72 | + .parameters |
| 73 | + .step_planner |
| 74 | + .optimization_parameters |
| 75 | + .optimizer_steps = optimizer_steps; |
| 76 | + |
| 77 | + println!("tick {}: {optimizer_steps} steps", time.ticks()); |
| 78 | + |
| 79 | + if time.ticks() >= 500 { |
| 80 | + exit.send(AppExit::Success); |
| 81 | + } |
| 82 | +} |
0 commit comments