Calculate step plan duration for each kick decision

Author: knoellle

crates/control/src/kick_selector.rs

@@ -1,31 +1,37 @@
-use std::cmp::Ordering;
+use std::{cmp::Ordering, time::Duration};
 
 use color_eyre::Result;
 use itertools::iproduct;
 use serde::{Deserialize, Serialize};
 
 use context_attribute::context;
-use coordinate_systems::{Field, Ground, UpcomingSupport};
+use coordinate_systems::{Field, Ground, Robot, UpcomingSupport};
 use framework::{AdditionalOutput, MainOutput};
 use geometry::{circle::Circle, line_segment::LineSegment, look_at::LookAt};
 use linear_algebra::{
-    distance, point, vector, IntoFramed, Isometry2, Orientation2, Point, Point2, Pose2, Rotation2,
-    Vector2,
+    distance, point, vector, IntoFramed, Isometry2, Isometry3, Orientation2, Orientation3, Point,
+    Point2, Point3, Pose2, Rotation2, Vector2, Vector3,
 };
 use spl_network_messages::{GamePhase, SubState, Team};
 use types::{
+    cycle_time::CycleTime,
     field_dimensions::{self, FieldDimensions, Half},
     filtered_game_controller_state::FilteredGameControllerState,
     filtered_game_state::FilteredGameState,
+    joints::body::BodyJoints,
     kick_decision::{DecisionParameters, KickDecision, PlayingSituation},
     motion_command::{KickVariant, OrientationMode, WalkSpeed},
+    obstacle_avoiding_arms::ArmCommands,
     obstacles::Obstacle,
     parameters::{InWalkKickInfoParameters, InWalkKicksParameters, StepPlannerParameters},
     planned_path::direct_path,
+    sensor_data::SensorData,
     support_foot::Side,
     world_state::BallState,
 };
-use walking_engine::mode::Mode;
+use walking_engine::{
+    kick_steps::KickSteps, mode::Mode, parameters::Parameters, step_plan::StepPlan, Context,
+};
 
 use crate::motion::step_planner::step_plan_greedy;
 
@@ -48,10 +54,21 @@ pub struct CycleContext {
 
     decision_parameters: Parameter<DecisionParameters, "kick_selector">,
     step_planner_parameters: Parameter<StepPlannerParameters, "step_planner">,
+    kick_steps: Parameter<KickSteps, "kick_steps">,
+    walking_engine_parameters: Parameter<Parameters, "walking_engine">,
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
     in_walk_kicks: Parameter<InWalkKicksParameters, "in_walk_kicks">,
     dribble_walk_speed: Parameter<WalkSpeed, "walk_speed.dribble">,
 
+    cycle_time: Input<CycleTime, "cycle_time">,
+    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">,
+
     playing_situation: AdditionalOutput<PlayingSituation, "playing_situation">,
 }
 
@@ -116,12 +133,46 @@ impl KickSelector {
             context.in_walk_kicks,
         );
 
+        let arm_compensation = 0.0;
+        let step_compensation = 0.0;
+        let robot_to_walk = Isometry3::from_parts(
+            vector![
+                context.walking_engine_parameters.base.torso_offset,
+                0.0,
+                context.walking_engine_parameters.base.walk_height,
+            ],
+            Orientation3::new(
+                Vector3::y_axis()
+                    * (context.walking_engine_parameters.base.torso_tilt_base
+                        + step_compensation
+                        + arm_compensation),
+            ),
+        );
+        let 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_to_ground: context.robot_to_ground,
+            gyro: nalgebra::Vector3::zeros(),
+            last_actuated_joints: BodyJoints::default(),
+            measured_joints: context.sensor_data.positions.into(),
+            robot_to_walk,
+            obstacle_avoiding_arms: &ArmCommands::default(),
+            zero_moment_point: context.zero_moment_point,
+            number_of_consecutive_cycles_zero_moment_point_outside_support_polygon: context
+                .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;
-                step_plan_greedy(
+                let step_plan = step_plan_greedy(
                     &direct_path(
                         Point2::origin(),
                         kick_pose_in_upcoming_support_ground.position(),
@@ -132,7 +183,30 @@ impl KickSelector {
                     OrientationMode::Override(kick_pose_in_upcoming_support_ground.orientation()),
                     *context.dribble_walk_speed,
                 )
-                .unwrap()
+                .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);

crates/walking_engine/src/lib.rs

@@ -22,7 +22,7 @@ mod kick_state;
 pub mod kick_steps;
 pub mod mode;
 pub mod parameters;
-mod step_plan;
+pub mod step_plan;
 mod step_state;
 mod stiffness;