Update docs for walking (#1439)

* rename file of step from step_plan to step * do nicer example for behavior * change attention to note since it's not that attentionmäßig * add docs about walking and disclaimer for step planning * Update docs/robotics/motion/step_planning.md Co-authored-by: knoellle <[email protected]> * Update docs/robotics/motion/walking.md Co-authored-by: knoellle <[email protected]> * Update docs/robotics/motion/walking.md Co-authored-by: knoellle <[email protected]> * Update docs/robotics/motion/walking.md Co-authored-by: knoellle <[email protected]> * better comment for step-planning * add links --------- Co-authored-by: knoellle <[email protected]>
HULKs · Oct 13, 2024 · 4cb6d40 · 4cb6d40
1 parent a4145e5
commit 4cb6d40
Show file tree

Hide file tree

Showing 28 changed files with 111 additions and 38 deletions.
diff --git a/crates/control/src/behavior/intercept_ball.rs b/crates/control/src/behavior/intercept_ball.rs
@@ -9,7 +9,7 @@ use types::{
     motion_command::{HeadMotion, ImageRegion, MotionCommand, OrientationMode, WalkSpeed},
     parameters::InterceptBallParameters,
     planned_path::PathSegment,
-    step_plan::Step,
+    step::Step,
     world_state::{BallState, WorldState},
 };
 

diff --git a/crates/control/src/behavior/node.rs b/crates/control/src/behavior/node.rs
@@ -23,7 +23,7 @@ use types::{
     planned_path::PathSegment,
     primary_state::PrimaryState,
     roles::Role,
-    step_plan::Step,
+    step::Step,
     world_state::WorldState,
 };
 

diff --git a/crates/control/src/motion/step_planner.rs b/crates/control/src/motion/step_planner.rs
@@ -8,7 +8,7 @@ use linear_algebra::{Isometry2, Orientation2, Pose2};
 use types::{
     motion_command::{MotionCommand, OrientationMode, WalkSpeed},
     planned_path::PathSegment,
-    step_plan::Step,
+    step::Step,
 };
 
 #[derive(Deserialize, Serialize)]
@@ -39,7 +39,7 @@ pub struct CycleContext {
 #[context]
 #[derive(Default)]
 pub struct MainOutputs {
-    pub step_plan: MainOutput<Step>,
+    pub planned_step: MainOutput<Step>,
 }
 
 impl StepPlanner {
@@ -61,7 +61,7 @@ impl StepPlanner {
             } => (path, orientation_mode, speed),
             _ => {
                 return Ok(MainOutputs {
-                    step_plan: Step {
+                    planned_step: Step {
                         forward: 0.0,
                         left: 0.0,
                         turn: 0.0,
@@ -143,7 +143,7 @@ impl StepPlanner {
         );
 
         Ok(MainOutputs {
-            step_plan: step.into(),
+            planned_step: step.into(),
         })
     }
 }

diff --git a/crates/control/src/motion/walk_manager.rs b/crates/control/src/motion/walk_manager.rs
@@ -5,7 +5,7 @@ use serde::{Deserialize, Serialize};
 use types::{
     motion_command::MotionCommand,
     motion_selection::{MotionSelection, MotionType},
-    step_plan::Step,
+    step::Step,
     walk_command::WalkCommand,
 };
 
@@ -19,7 +19,7 @@ pub struct CreationContext {}
 pub struct CycleContext {
     motion_command: Input<MotionCommand, "motion_command">,
     motion_selection: Input<MotionSelection, "motion_selection">,
-    step_plan: Input<Step, "step_plan">,
+    planned_step: Input<Step, "planned_step">,
 }
 
 #[context]
@@ -42,7 +42,7 @@ impl WalkManager {
 
         let command = match context.motion_command {
             MotionCommand::Walk { .. } => WalkCommand::Walk {
-                step: *context.step_plan,
+                step: *context.planned_step,
             },
             MotionCommand::InWalkKick {
                 kick,

diff --git a/crates/types/src/lib.rs b/crates/types/src/lib.rs
@@ -67,7 +67,7 @@ pub mod sensor_data;
 pub mod sole_pressure;
 pub mod sonar_obstacle;
 pub mod sonar_values;
-pub mod step_plan;
+pub mod step;
 pub mod support_foot;
 pub mod walk_command;
 pub mod whistle;

diff --git a/crates/types/src/parameters.rs b/crates/types/src/parameters.rs
@@ -10,7 +10,7 @@ use crate::{
     joints::head::HeadJoints,
     motion_command::{KickVariant, MotionCommand},
     roles::Role,
-    step_plan::Step,
+    step::Step,
 };
 
 #[derive(

diff --git a/crates/types/src/step_plan.rs → crates/types/src/step.rs b/crates/types/src/step_plan.rs → crates/types/src/step.rs
diff --git a/crates/types/src/walk_command.rs b/crates/types/src/walk_command.rs
@@ -1,7 +1,7 @@
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 
-use crate::{motion_command::KickVariant, step_plan::Step, support_foot::Side};
+use crate::{motion_command::KickVariant, step::Step, support_foot::Side};
 
 #[derive(
     Default,

diff --git a/crates/walking_engine/src/anatomic_constraints.rs b/crates/walking_engine/src/anatomic_constraints.rs
@@ -1,4 +1,4 @@
-use types::{step_plan::Step, support_foot::Side};
+use types::{step::Step, support_foot::Side};
 
 pub trait AnatomicConstraints {
     fn clamp_to_anatomic_constraints(self, support_side: Side, maximum_inside_turn: f32) -> Step;

diff --git a/crates/walking_engine/src/feet.rs b/crates/walking_engine/src/feet.rs
@@ -3,7 +3,7 @@ use kinematics::forward::{left_sole_to_robot, right_sole_to_robot};
 use linear_algebra::{point, Isometry3, Orientation3, Pose3, Vector2, Vector3};
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
-use types::{joints::body::BodyJoints, step_plan::Step, support_foot::Side};
+use types::{joints::body::BodyJoints, step::Step, support_foot::Side};
 
 use crate::parameters::Parameters;
 

diff --git a/crates/walking_engine/src/kick_steps.rs b/crates/walking_engine/src/kick_steps.rs
@@ -2,7 +2,7 @@ use std::time::Duration;
 
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
-use types::{motion_command::KickVariant, step_plan::Step};
+use types::{motion_command::KickVariant, step::Step};
 
 #[derive(Clone, Copy, Debug, Serialize, Deserialize)]
 pub struct JointOverride {

diff --git a/crates/walking_engine/src/lib.rs b/crates/walking_engine/src/lib.rs
@@ -12,7 +12,7 @@ use serde::{Deserialize, Serialize};
 use types::{
     cycle_time::CycleTime, joints::body::BodyJoints, motion_command::KickVariant,
     motor_commands::MotorCommands, obstacle_avoiding_arms::ArmCommands,
-    sensor_data::ForceSensitiveResistors, step_plan::Step, support_foot::Side,
+    sensor_data::ForceSensitiveResistors, step::Step, support_foot::Side,
 };
 
 mod anatomic_constraints;

diff --git a/crates/walking_engine/src/mode.rs b/crates/walking_engine/src/mode.rs
@@ -2,7 +2,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 use crate::{Context, WalkTransition};

diff --git a/crates/walking_engine/src/mode/catching.rs b/crates/walking_engine/src/mode/catching.rs
@@ -17,7 +17,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 #[derive(

diff --git a/crates/walking_engine/src/mode/kicking.rs b/crates/walking_engine/src/mode/kicking.rs
@@ -14,7 +14,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 #[derive(

diff --git a/crates/walking_engine/src/mode/standing.rs b/crates/walking_engine/src/mode/standing.rs
@@ -4,7 +4,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 use crate::{

diff --git a/crates/walking_engine/src/mode/starting.rs b/crates/walking_engine/src/mode/starting.rs
@@ -8,7 +8,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 #[derive(

diff --git a/crates/walking_engine/src/mode/stopping.rs b/crates/walking_engine/src/mode/stopping.rs
@@ -8,7 +8,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 #[derive(

diff --git a/crates/walking_engine/src/mode/walking.rs b/crates/walking_engine/src/mode/walking.rs
@@ -3,7 +3,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 
 use crate::{

diff --git a/crates/walking_engine/src/parameters.rs b/crates/walking_engine/src/parameters.rs
@@ -6,7 +6,7 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 use types::{
     joints::{arm::ArmJoints, leg::LegJoints},
-    step_plan::Step,
+    step::Step,
 };
 
 #[derive(

diff --git a/crates/walking_engine/src/step_plan.rs b/crates/walking_engine/src/step_plan.rs
@@ -4,7 +4,7 @@ use coordinate_systems::Walk;
 use linear_algebra::Vector2;
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
-use types::{step_plan::Step, support_foot::Side};
+use types::{step::Step, support_foot::Side};
 
 use crate::Context;
 

diff --git a/docs/robotics/behavior/overview.md b/docs/robotics/behavior/overview.md
@@ -8,11 +8,40 @@ This is organized in two steps:
 
 In this step, the world state and other inputs are used to create a priority sorted list of actions, which we would like to take.
 
--   Examples: `Unstiff`, `SitDown`, `Penalize`, `Initial`, `FallSafely`, `StandUp`, ...
+!!! example
+
+    ```rust
+    let mut actions = vec![
+                Action::Unstiff,
+                Action::SitDown,
+                Action::Penalize,
+                Action::Initial,
+                Action::FallSafely,
+                Action::StandUp,
+            ];
+
+    ```
 
 Depending on the current situation, different actions are added to the list of available actions.
 
--   Example: If the current role is keeper and a penalty shootout is happening, the actions `Jump` and `PrepareJump` are added.
+!!! example
+
+    If the current role is keeper and a penalty shootout is happening, the actions `Jump` and `PrepareJump` are added.
+
+    ``` rust
+        match world_state.robot.role {
+            Role::Keeper => match world_state.filtered_game_controller_state {
+                Some(FilteredGameControllerState {
+                    game_phase: GamePhase::PenaltyShootout { .. },
+                    ..
+                }) => {
+                    actions.push(Action::Jump);
+                    actions.push(Action::PrepareJump);
+                }
+                _ => actions.push(Action::DefendGoal),
+            },
+            ...
+    ```
 
 ## Action Selection
 

diff --git a/docs/robotics/motion/overview.md b/docs/robotics/motion/overview.md
@@ -1,6 +1,6 @@
 # Overview
 
-!!! warning
+!!! note
 
     This structure in three steps is only conceptionally, in the code, there is no differentiation between these steps, and all nodes are automatically sorted during compile time depending on their inputs and outputs.
 

diff --git a/docs/robotics/motion/step_planning.md b/docs/robotics/motion/step_planning.md
@@ -0,0 +1,5 @@
+!!! warning
+
+    The step planning is currently under development and will be redesigned by [Narcha](https://github.com/Narcha).
+
+    At the time of writing, only individual steps are planned, but not the whole step sequence, which could lead to various improvements, such as an increase in speed when planning around obstacles.
diff --git a/docs/robotics/motion/walking.md b/docs/robotics/motion/walking.md
@@ -0,0 +1,30 @@
+The whole process to let the robot walk is organized in three steps.
+
+1.  The step planner creates the planned step.
+    This includes x, y, and rotation depending on the motion command.
+
+    !!! warning
+
+        The step planning is currently under development and will be redesigned by [Narcha](https://github.com/Narcha).
+
+        At the time of writing, only individual steps are planned, but not the whole step sequence.
+        This is subject to change.
+
+2.  The walk manager uses the planned step and the motion command to create the walk command, which defines the walking mode, such as standing, walking, or others.
+
+3.  The walking engine uses the walk command and computes the according motor comamands.
+
+## Walking Engine
+
+The core of the walking engine is based on an idea by [Bernhard Hengst](https://www.researchgate.net/profile/Bernhard-Hengst) from ([UNSW Sydney](https://www.unsw.edu.au/)), which is used by almost all RoboCup SPL teams.<br>
+The idea is quite simple:
+
+-   There are two feet, a _support foot_ and a _swing foot_
+-   Move the swing foot forward with speed $x^2$
+-   Move the support foot backward with speed $x$.
+
+But around this, there's a lot of state handling and transitions.
+
+!!! todo
+
+    Add a diagram of the walking engine
diff --git a/mkdocs.yml b/mkdocs.yml
@@ -106,3 +106,9 @@ markdown_extensions:
       alternate_style: true
   - pymdownx.superfences
   - pymdownx.tilde
+  - pymdownx.arithmatex:
+      generic: true
+
+extra_javascript:
+  - javascripts/mathjax.js
+  - https://unpkg.com/mathjax@3/es5/tex-mml-chtml.js
diff --git a/tools/twix/src/panels/map/layers/walking.rs b/tools/twix/src/panels/map/layers/walking.rs
@@ -7,7 +7,7 @@ use coordinate_systems::{Ground, Robot, UpcomingSupport, Walk};
 use linear_algebra::{point, Isometry2, Isometry3, Point2, Point3, Pose2, Pose3};
 use types::{
     field_dimensions::FieldDimensions, joints::body::BodyJoints, robot_kinematics::RobotKinematics,
-    step_plan::Step, support_foot::Side,
+    step::Step, support_foot::Side,
 };
 use walking_engine::{
     mode::{
@@ -26,7 +26,7 @@ pub struct Walking {
     robot_kinematics: BufferHandle<RobotKinematics>,
     walking_engine: BufferHandle<Option<Engine>>,
     last_actuated_joints: BufferHandle<Option<BodyJoints>>,
-    step_plan: BufferHandle<Step>,
+    planned_step: BufferHandle<Step>,
     center_of_mass: BufferHandle<Point3<Robot>>,
     robot_to_walk: BufferHandle<Option<Isometry3<Robot, Walk>>>,
     zero_moment_point: BufferHandle<Point2<Ground>>,
@@ -43,7 +43,7 @@ impl Layer<Ground> for Walking {
         let walking_engine = nao.subscribe_value("Control.additional_outputs.walking.engine");
         let last_actuated_joints =
             nao.subscribe_value("Control.additional_outputs.walking.last_actuated_joints");
-        let step_plan = nao.subscribe_value("Control.main_outputs.step_plan");
+        let planned_step = nao.subscribe_value("Control.main_outputs.planned_step");
         let center_of_mass = nao.subscribe_value("Control.main_outputs.center_of_mass");
         let robot_to_walk = nao.subscribe_value("Control.additional_outputs.walking.robot_to_walk");
         let zero_moment_point = nao.subscribe_value("Control.main_outputs.zero_moment_point");
@@ -54,7 +54,7 @@ impl Layer<Ground> for Walking {
             robot_kinematics,
             walking_engine,
             last_actuated_joints,
-            step_plan,
+            planned_step,
             center_of_mass,
             robot_to_walk,
             zero_moment_point,
@@ -84,7 +84,7 @@ impl Layer<Ground> for Walking {
         else {
             return Ok(());
         };
-        let Some(step_plan) = self.step_plan.get_last_value()? else {
+        let Some(planned_step) = self.planned_step.get_last_value()? else {
             return Ok(());
         };
         let Some(center_of_mass) = self.center_of_mass.get_last_value()? else {
@@ -150,7 +150,7 @@ impl Layer<Ground> for Walking {
             Stroke::new(0.001, Color32::BLACK),
         );
 
-        paint_step_plan(painter, step_plan, ground_to_upcoming_support);
+        paint_planned_step(painter, planned_step, ground_to_upcoming_support);
         Ok(())
     }
 }
@@ -327,9 +327,9 @@ fn paint_sole_polygon(
     );
 }
 
-fn paint_step_plan(
+fn paint_planned_step(
     painter: &TwixPainter<Ground>,
-    step_plan: Step,
+    planned_step: Step,
     ground_to_upcoming_support: Isometry2<Ground, UpcomingSupport>,
 ) {
     painter.pose(
@@ -341,7 +341,10 @@ fn paint_step_plan(
     );
     painter.pose(
         ground_to_upcoming_support.inverse()
-            * Pose2::new(point![step_plan.forward, step_plan.left], step_plan.turn),
+            * Pose2::new(
+                point![planned_step.forward, planned_step.left],
+                planned_step.turn,
+            ),
         0.02,
         0.03,
         Color32::RED,