Forward kinematic (#2116)

* start K1 joints

* change joints to booster, remove mio

* remove unused coordinate systems

* add center of masses, masses

* check vivis errors

* rename robotdimensions

* check robot positions

* add more robot positions

* rename legdimensions

* dO Do TODO formating

* fix righ sole to robot

* limprojector to new arms

* delete inverse kinematic and --> walking engine

* work on todos

* add missing translations

* work on review comment, fix duplicate rotation error

---------

Co-authored-by: Johannes Blum <johannes.blum@tuhh.de>

Author: vivi-enne

Cargo.lock

@@ -47,12 +47,10 @@ members = [
   "crates/step_planning_solver",
   "crates/types",
   "crates/vision",
-  "crates/walking_engine",
   "crates/zed",
   "tools/annotato",
   "tools/depp",
   "tools/fanta",
-  "tools/mio",
   "tools/mujoco-simulator/mujoco-rust-server",
   "tools/parameter_tester",
   "tools/pepsi",
@@ -246,7 +244,6 @@ uuid = { version = "1.12.1", features = ["v4"] }
 v4l = { version = "0.12.1", git = "https://github.com/HULKs/libv4l-rs", rev = "be65819073514b193d082dd37dbcc2cfac3f6183" }
 vision = { path = "crates/vision" }
 walkdir = "2.5.0"
-walking_engine = { path = "crates/walking_engine" }
 watch = "0.2.3"
 webots = { version = "0.8.0" }
 xdg = "2.5.2"

Cargo.toml

@@ -48,7 +48,6 @@ tokio = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }
 vision = { workspace = true }
-walking_engine = { workspace = true }
 zed = { workspace = true }
 
 [build-dependencies]

crates/bevyhavior_simulator/Cargo.toml

@@ -30,9 +30,6 @@ fn main() -> Result<()> {
                     "control::kinematics_provider",
                     "control::motion::look_around",
                     "control::motion::motion_selector",
-                    "control::motion::step_planner",
-                    "control::motion::walking_engine",
-                    "control::motion::walk_manager",
                     "control::odometry",
                     "control::penalty_shot_direction_estimation",
                     "control::primary_state_filter",

crates/bevyhavior_simulator/build.rs

@@ -3,7 +3,7 @@ use bevy::{ecs::system::SystemParam, prelude::*};
 use linear_algebra::{point, vector, Isometry2, Point2, Vector};
 use scenario::scenario;
 use spl_network_messages::{GameState, PlayerNumber};
-use types::{ball_position::SimulatorBallState, step::Step};
+use types::ball_position::SimulatorBallState;
 
 use bevyhavior_simulator::{
     ball::BallResource,
@@ -33,13 +33,6 @@ 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.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,
-        turn: 1.0,
-    };
     commands.spawn(robot);
     game_controller.state.game_state = GameState::Playing;
     game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Playing));

crates/bevyhavior_simulator/src/bin/intercept_ball.rs

@@ -67,12 +67,6 @@ fn update(
     });
 
     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());
 

crates/bevyhavior_simulator/src/bin/mpc_step_planning_optimizer.rs

@@ -32,16 +32,6 @@ fn startup(
 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;

crates/bevyhavior_simulator/src/bin/step_planning_test.rs

@@ -1,6 +1,5 @@
 use std::{
     convert::Into,
-    f32::consts::FRAC_PI_2,
     mem::take,
     sync::{mpsc, Arc},
     time::{Duration, SystemTime, UNIX_EPOCH},
@@ -17,14 +16,13 @@ use bevy::{
 use color_eyre::{eyre::WrapErr, Result};
 
 use buffered_watch::{Receiver, Sender};
-use control::{localization::generate_initial_pose, zero_moment_point_provider::LEFT_FOOT_OUTLINE};
+use control::localization::generate_initial_pose;
 use coordinate_systems::{Field, Ground, Head, LeftSole, RightSole, Robot as RobotCoordinates};
 use framework::{future_queue, Producer, RecordingTrigger};
-use geometry::{circle::Circle, polygon::circle_overlaps_polygon};
 use hula_types::hardware::Ids;
 use linear_algebra::{
-    point, vector, Isometry2, Isometry3, Orientation2, Orientation3, Point2, Pose2, Pose3,
-    Rotation2, Vector2,
+    vector, Isometry2, Isometry3, Orientation2, Orientation3, Point2, Pose2, Pose3, Rotation2,
+    Vector2,
 };
 use parameters::directory::deserialize;
 use projection::intrinsic::Intrinsic;
@@ -34,17 +32,13 @@ use types::{
     filtered_whistle::FilteredWhistle,
     joints::Joints,
     messages::{IncomingMessage, OutgoingMessage},
-    motion_command::{HeadMotion, KickVariant},
+    motion_command::HeadMotion,
     motion_selection::MotionSafeExits,
     pose_kinds::PoseKind,
     robot_dimensions::RobotDimensions,
     sensor_data::Foot,
     support_foot::Side,
 };
-use walking_engine::{
-    kick_state::KickState,
-    mode::{kicking::Kicking, Mode},
-};
 
 use crate::{
     ball::BallResource,
@@ -249,65 +243,12 @@ pub fn from_player_number(val: PlayerNumber) -> usize {
     }
 }
 
-pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut<BallResource>, time: Res<Time>) {
+pub fn move_robots(mut robots: Query<&mut Robot>, _ball: ResMut<BallResource>, time: Res<Time>) {
     for mut robot in &mut robots {
         if let Some(ball) = robot.database.main_outputs.ball_position.as_mut() {
             ball.position += ball.velocity * time.delta_secs();
             ball.velocity *= 0.98
         }
-        if let Mode::Kicking(Kicking {
-            kick:
-                KickState {
-                    variant,
-                    side: kicking_side,
-                    strength,
-                    ..
-                },
-            ..
-        }) = robot.cycler.cycler_state.walking_engine_mode
-        {
-            if let Some(ball) = ball.state.as_mut() {
-                let side = match kicking_side {
-                    Side::Left => -1.0,
-                    Side::Right => 1.0,
-                };
-
-                let robot_to_ground = robot.database.main_outputs.robot_to_ground.unwrap();
-                let kinematics = &robot.database.main_outputs.robot_kinematics;
-                let left_sole_to_ground = robot_to_ground * kinematics.left_leg.sole_to_robot;
-                let right_sole_to_ground = robot_to_ground * kinematics.right_leg.sole_to_robot;
-                let left_sole_in_ground: Vec<_> = LEFT_FOOT_OUTLINE
-                    .into_iter()
-                    .map(|point| (left_sole_to_ground * point).xy())
-                    .collect();
-                let right_sole_in_ground: Vec<_> = LEFT_FOOT_OUTLINE
-                    .into_iter()
-                    .map(|point| {
-                        (right_sole_to_ground * point![point.x(), -point.y(), point.z()]).xy()
-                    })
-                    .collect();
-
-                let ball_circle = Circle::new(
-                    robot.ground_to_field().inverse() * ball.position,
-                    robot.parameters.field_dimensions.ball_radius,
-                );
-                let in_range = circle_overlaps_polygon(&left_sole_in_ground, ball_circle)
-                    || circle_overlaps_polygon(&right_sole_in_ground, ball_circle);
-                let previous_kick_finished =
-                    (time.elapsed() - robot.last_kick_time).as_secs_f32() > 1.0;
-                if in_range && previous_kick_finished {
-                    let direction = match variant {
-                        KickVariant::Forward => Orientation2::identity(),
-                        KickVariant::Turn => Orientation2::new(0.35),
-                        KickVariant::Side => Orientation2::new(-FRAC_PI_2),
-                    }
-                    .as_unit_vector()
-                    .component_mul(&vector![1.0, side]);
-                    ball.velocity += robot.ground_to_field() * direction * strength * 2.5;
-                    robot.last_kick_time = time.elapsed();
-                };
-            }
-        };
 
         let (left_sole, right_sole) =
             sole_positions(&robot.database.main_outputs.sensor_data.positions);
@@ -324,17 +265,6 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut<BallResource>
         let ground = robot.database.main_outputs.robot_to_ground.unwrap() * support_sole;
         let anchor = robot.ground_to_field() * to2d(ground);
 
-        let target = robot.database.main_outputs.walk_motor_commands.positions;
-        let positions = &mut robot.database.main_outputs.sensor_data.positions;
-        positions.left_leg =
-            positions.left_leg + (target.left_leg - positions.left_leg) * time.delta_secs() * 10.0;
-        positions.right_leg = positions.right_leg
-            + (target.right_leg - positions.right_leg) * time.delta_secs() * 10.0;
-        positions.left_arm =
-            positions.left_arm + (target.left_arm - positions.left_arm) * time.delta_secs() * 10.0;
-        positions.right_arm = positions.right_arm
-            + (target.right_arm - positions.right_arm) * time.delta_secs() * 10.0;
-
         let (new_left_sole, new_right_sole) =
             sole_positions(&robot.database.main_outputs.sensor_data.positions);
         let support_sole = match support_foot {
@@ -505,12 +435,7 @@ pub fn cycle_robots(
             .support_foot
             .support_side
             .unwrap();
-        let is_step_finished = robot
-            .cycler
-            .cycler_state
-            .walking_engine_mode
-            .step_state()
-            .is_some_and(|step_state| step_state.time_since_start >= step_state.plan.step_duration);
+        let is_step_finished = false;
         let next_support_foot = if is_step_finished {
             support_foot.opposite()
         } else {
@@ -568,7 +493,7 @@ fn sole_positions(joint_positions: &Joints) -> (Pose3<RobotCoordinates>, Pose3<R
     let left_foot_to_robot =
         left_ankle_to_robot * left_foot_to_left_ankle(&joint_positions.left_leg);
     let left_sole_to_robot: Isometry3<LeftSole, RobotCoordinates> =
-        left_foot_to_robot * Isometry3::from(RobotDimensions::LEFT_ANKLE_TO_LEFT_SOLE);
+        left_foot_to_robot * Isometry3::from(RobotDimensions::LEFT_FOOT_TO_LEFT_SOLE);
     // right leg
     let right_pelvis_to_robot = right_pelvis_to_robot(&joint_positions.right_leg);
     let right_hip_to_robot =
@@ -582,7 +507,7 @@ fn sole_positions(joint_positions: &Joints) -> (Pose3<RobotCoordinates>, Pose3<R
     let right_foot_to_robot =
         right_ankle_to_robot * right_foot_to_right_ankle(&joint_positions.right_leg);
     let right_sole_to_robot: Isometry3<RightSole, RobotCoordinates> =
-        right_foot_to_robot * Isometry3::from(RobotDimensions::RIGHT_ANKLE_TO_RIGHT_SOLE);
+        right_foot_to_robot * Isometry3::from(RobotDimensions::RIGHT_FOOT_TO_RIGHT_SOLE);
 
     (left_sole_to_robot.as_pose(), right_sole_to_robot.as_pose())
 }

crates/bevyhavior_simulator/src/robot.rs

@@ -42,4 +42,3 @@ splines = { workspace = true }
 step_planning = { workspace = true }
 step_planning_solver = { workspace = true }
 types = { workspace = true }
-walking_engine = { workspace = true }

crates/control/Cargo.toml

@@ -23,7 +23,6 @@ use types::{
     limb::{is_above_limbs, Limb, ProjectedLimbs},
     parameters::BallFilterParameters,
 };
-use walking_engine::mode::Mode;
 
 #[derive(Deserialize, Serialize)]
 pub struct BallFilter {
@@ -54,7 +53,6 @@ pub struct CycleContext {
 
     balls: PerceptionInput<Option<Vec<BallPercept>>, "Vision", "balls?">,
     projected_limbs: PerceptionInput<Option<ProjectedLimbs>, "Vision", "projected_limbs?">,
-    walking_engine_mode: CyclerState<Mode, "walking_engine_mode">,
 }
 
 #[context]
@@ -83,7 +81,6 @@ impl BallFilter {
         filter_parameters: &BallFilterParameters,
         field_dimensions: &FieldDimensions,
         cycle_time: &CycleTime,
-        walking_engine_mode: Mode,
     ) {
         for (detection_time, balls) in measurements {
             self.ball_filter.hypotheses.retain(|hypothesis| {
@@ -183,11 +180,9 @@ impl BallFilter {
                 .expect("time ran backwards");
             let validity_high_enough =
                 hypothesis.validity >= filter_parameters.validity_discard_threshold;
-            let ball_kicked = matches!(walking_engine_mode, Mode::Kicking(_));
             is_ball_inside_field(ball, field_dimensions)
                 && validity_high_enough
                 && duration_since_last_observation < filter_parameters.hypothesis_timeout
-                && !ball_kicked
         };
 
         let should_merge_hypotheses =
@@ -225,7 +220,6 @@ impl BallFilter {
             filter_parameters,
             context.field_dimensions,
             context.cycle_time,
-            *context.walking_engine_mode,
         );
 
         context