Use time to reach kick pose for sorting

Author: knoellle

crates/control/src/kick_selector.rs

@@ -26,11 +26,13 @@ use types::{
     parameters::{InWalkKickInfoParameters, InWalkKicksParameters, StepPlannerParameters},
     planned_path::direct_path,
     sensor_data::SensorData,
+    step::Step,
     support_foot::Side,
     world_state::BallState,
 };
 use walking_engine::{
-    kick_steps::KickSteps, mode::Mode, parameters::Parameters, step_plan::StepPlan, Context,
+    kick_steps::KickSteps, mode::Mode, parameters::Parameters, step_plan::StepPlan,
+    step_state::StepState, Context,
 };
 
 use crate::motion::step_planner::step_plan_greedy;
@@ -64,7 +66,6 @@ pub struct CycleContext {
     center_of_mass: Input<Point3<Robot>, "center_of_mass">,
     sensor_data: Input<SensorData, "sensor_data">,
     robot_to_ground: Input<Option<Isometry3<Robot, Ground>>, "robot_to_ground?">,
-    robot_orientation: RequiredInput<Option<Orientation3<Field>>, "robot_orientation?">,
     zero_moment_point: Input<Point2<Ground>, "zero_moment_point">,
     number_of_consecutive_cycles_zero_moment_point_outside_support_polygon:
         Input<i32, "number_of_consecutive_cycles_zero_moment_point_outside_support_polygon">,
@@ -148,17 +149,17 @@ impl KickSelector {
                         + arm_compensation),
             ),
         );
-        let walking_engine_context = Context {
+        let mut walking_engine_context = Context {
             parameters: context.walking_engine_parameters,
             max_step_size: &context.step_planner_parameters.max_step_size,
             kick_steps: context.kick_steps,
             cycle_time: context.cycle_time,
             center_of_mass: context.center_of_mass,
             force_sensitive_resistors: &context.sensor_data.force_sensitive_resistors,
-            robot_orientation: context.robot_orientation,
+            robot_orientation: &Orientation3::default(),
             robot_to_ground: context.robot_to_ground,
             gyro: nalgebra::Vector3::zeros(),
-            last_actuated_joints: BodyJoints::default(),
+            last_actuated_joints: BodyJoints::default(), // TODO: Crude approximation
             measured_joints: context.sensor_data.positions.into(),
             robot_to_walk,
             obstacle_avoiding_arms: &ArmCommands::default(),
@@ -167,56 +168,60 @@ impl KickSelector {
                 .number_of_consecutive_cycles_zero_moment_point_outside_support_polygon,
         };
 
-        let step_plans: Vec<_> = kick_decisions
-            .iter()
-            .map(|kick_decision| {
-                let kick_pose_in_upcoming_support_ground =
-                    *context.ground_to_upcoming_support * kick_decision.kick_pose;
-                let step_plan = step_plan_greedy(
-                    &direct_path(
-                        Point2::origin(),
-                        kick_pose_in_upcoming_support_ground.position(),
-                    ),
-                    context.step_planner_parameters,
-                    Pose2::zero(),
-                    *context.walking_engine_mode,
-                    OrientationMode::Override(kick_pose_in_upcoming_support_ground.orientation()),
-                    *context.dribble_walk_speed,
-                )
-                .unwrap();
-
-                let support_side = context
-                    .walking_engine_mode
-                    .support_side()
-                    .unwrap_or(Side::Left);
-                let walk_duration: Duration = step_plan
-                    .iter()
-                    .map(|step| {
-                        let step_plan = StepPlan::new_from_request(
-                            &walking_engine_context,
-                            *step,
-                            support_side,
-                        );
-                        step_plan.step_duration
-                    })
-                    .sum();
-
-                let distance = distance_to_kick_pose(
-                    *context.ground_to_upcoming_support * kick_decision.kick_pose,
-                    context.decision_parameters.angle_distance_weight,
-                );
-
-                (distance, walk_duration)
-            })
-            .collect();
-        dbg!(step_plans);
+        let step = match *context.walking_engine_mode {
+            Mode::Walking(walking) => walking.requested_step,
+            _ => Step::default(),
+        };
+        let current_support_side = context
+            .walking_engine_mode
+            .support_side()
+            .unwrap_or(Side::Left);
+        let step_plan =
+            StepPlan::new_from_request(&walking_engine_context, step, current_support_side);
+
+        walking_engine_context.last_actuated_joints =
+            calculate_joints_at_end_of_step(step_plan, &walking_engine_context);
+
+        let rate_kick_decision = |kick_decision: &KickDecision| {
+            let kick_pose_in_upcoming_support_ground =
+                *context.ground_to_upcoming_support * kick_decision.kick_pose;
+            let step_plan = step_plan_greedy(
+                &direct_path(
+                    Point2::origin(),
+                    kick_pose_in_upcoming_support_ground.position(),
+                ),
+                context.step_planner_parameters,
+                Pose2::zero(),
+                current_support_side.opposite(),
+                OrientationMode::Override(kick_pose_in_upcoming_support_ground.orientation()),
+                *context.dribble_walk_speed,
+            )
+            .unwrap();
+            let mut walking_engine_context = walking_engine_context.clone();
+            let walk_duration: Duration = step_plan
+                .iter()
+                .map(|(step, support_side)| {
+                    let step_plan =
+                        StepPlan::new_from_request(&walking_engine_context, *step, *support_side);
+                    walking_engine_context.last_actuated_joints =
+                        calculate_joints_at_end_of_step(step_plan, &walking_engine_context);
+                    step_plan.step_duration
+                })
+                .sum();
+
+            walk_duration
+        };
+
+        // let step_plans: Vec<_> = kick_decisions.iter().map(rate_kick_decision).collect();
+        // dbg!(step_plans);
 
         kick_decisions.sort_by(|left, right| {
             compare_decisions(
                 left,
                 right,
+                rate_kick_decision(left),
+                rate_kick_decision(right),
                 ball_position,
-                *context.ground_to_upcoming_support,
                 context.obstacles,
                 context.decision_parameters,
             )
@@ -236,8 +241,9 @@ impl KickSelector {
             compare_decisions(
                 left,
                 right,
+                rate_kick_decision(left),
+                rate_kick_decision(right),
                 ball_position,
-                *context.ground_to_upcoming_support,
                 context.obstacles,
                 context.decision_parameters,
             )
@@ -250,6 +256,16 @@ impl KickSelector {
     }
 }
 
+fn calculate_joints_at_end_of_step(
+    step_plan: StepPlan,
+    walking_engine_context: &Context<'_>,
+) -> BodyJoints {
+    let mut step_state = StepState::new(step_plan);
+    step_state.time_since_start = step_plan.step_duration;
+
+    step_state.compute_joints(walking_engine_context)
+}
+
 fn is_ball_in_opponents_corners(
     ball_position: Point2<Field>,
     field_dimensions: &FieldDimensions,
@@ -374,8 +390,9 @@ fn generate_penalty_shot_kick_targets(context: &CycleContext) -> Vec<Point2<Grou
 fn compare_decisions(
     left: &KickDecision,
     right: &KickDecision,
+    time_to_reach_left: Duration,
+    time_to_reach_right: Duration,
     ball_position: Point2<Ground>,
-    ground_to_upcoming_support: Isometry2<Ground, UpcomingSupport>,
     obstacles: &[Obstacle],
     parameters: &DecisionParameters,
 ) -> Ordering {
@@ -385,14 +402,6 @@ fn compare_decisions(
         is_intersecting_with_an_obstacle(obstacles, ball_position, left.target, parameters);
     let right_is_intersecting_with_obstacle =
         is_intersecting_with_an_obstacle(obstacles, ball_position, right.target, parameters);
-    let distance_to_left = distance_to_kick_pose(
-        ground_to_upcoming_support * left.kick_pose,
-        parameters.angle_distance_weight,
-    );
-    let distance_to_right = distance_to_kick_pose(
-        ground_to_upcoming_support * right.kick_pose,
-        parameters.angle_distance_weight,
-    );
 
     match (
         left_in_obstacle,
@@ -404,7 +413,7 @@ fn compare_decisions(
         (true, false, _, _) => Ordering::Greater,
         (_, _, false, true) => Ordering::Less,
         (_, _, true, false) => Ordering::Greater,
-        _ => distance_to_left.total_cmp(&distance_to_right),
+        _ => time_to_reach_left.cmp(&time_to_reach_right),
     }
 }
 

crates/control/src/motion/step_planner.rs

@@ -206,16 +206,13 @@ pub fn step_plan_greedy<Frame>(
     path: &[PathSegment<Frame>],
     parameters: &StepPlannerParameters,
     mut pose: Pose2<Frame>,
-    walking_engine_mode: Mode,
+    initial_support_side: Side,
     orientation_mode: OrientationMode<Frame>,
     speed: WalkSpeed,
-) -> Result<Vec<Step>> {
+) -> Result<Vec<(Step, Side)>> {
     let mut steps = Vec::new();
     let mut last_planned_step = Step::default();
-    let mut support_side = walking_engine_mode
-        .support_side()
-        .unwrap_or(Side::Left)
-        .opposite();
+    let mut support_side = initial_support_side;
 
     let destination = match path.last() {
         Some(PathSegment::Arc(arc)) => arc.end_point(),
@@ -278,10 +275,9 @@ pub fn step_plan_greedy<Frame>(
 
         pose = pose.as_transform() * step_rotation * step_translation.as_pose();
         last_planned_step = step;
+        steps.push((step, support_side));
         support_side = support_side.opposite();
 
-        steps.push(step);
-
         if (destination - pose.position()).norm_squared() > 0.01 {
             continue;
         }

crates/walking_engine/src/lib.rs

@@ -23,14 +23,15 @@ pub mod kick_steps;
 pub mod mode;
 pub mod parameters;
 pub mod step_plan;
-mod step_state;
+pub mod step_state;
 mod stiffness;
 
 /// # WalkingEngine
 /// This node generates foot positions and thus leg angles for the robot to execute a walk.
 /// The algorithm to compute the feet trajectories is loosely based on the work of Bernhard Hengst
 /// at the team rUNSWift. An explanation of this algorithm can be found in the team's research
 /// report from 2014 (<http://cgi.cse.unsw.edu.au/~robocup/2014ChampionTeamPaperReports/20140930-Bernhard.Hengst-Walk2014Report.pdf>).
+#[derive(Debug, Clone)]
 pub struct Context<'a> {
     pub parameters: &'a Parameters,
     pub max_step_size: &'a Step,