Documentation and Flowchart for the Walking Engine

[CodeByDamianK]

Problem:
Documentation and ideally a flowchart is needed to better understand the walking engine modules/code and how its modules and submodules interact with each other.

Task:
Writing a documentation and creating a flowchart about the walking engine.

---

Reference: crates/walking_engine/src
Current version of the walking engine modules and code (Copied below for faster access):

Here are the following walking_engine modules:

anatomic_constraints.rs

arm.rs

feet.rs

foot_leveling.rs

gyro_balancing.rs

kick_state.rs

kick_steps.rs

lib.rs

mode.rs

parameters.rs

step_plan.rs

step_state.rs

stiffness.rs

mode

the module mode has also the following sub modules:
catching.rs

kicking.rs

standing.rs

starting.rs

stopping.rs

walking.rs

the following code is in anatomic_constraints.rs :
use types::{step::Step, support_foot::Side};

pub trait AnatomicConstraints {
fn clamp_to_anatomic_constraints(self, support_side: Side, maximum_inside_turn: f32) -> Step;
}

impl AnatomicConstraints for Step {
fn clamp_to_anatomic_constraints(self, support_side: Side, maximum_inside_turn: f32) -> Step {
let sideways_direction = if self.left.is_sign_positive() {
Side::Left
} else {
Side::Right
};
let clamped_left = if sideways_direction == support_side {
0.0
} else {
self.left
};
let turn_direction = if self.turn.is_sign_positive() {
Side::Left
} else {
Side::Right
};
let clamped_turn = if turn_direction == support_side {
self.turn.clamp(-maximum_inside_turn, maximum_inside_turn)
} else {
self.turn
};
Step {
forward: self.forward,
left: clamped_left,
turn: clamped_turn,
}
}
}

the following code is in arm.rs :
use splines::Interpolate as _;
use types::{
joints::{arm::ArmJoints, body::BodyJoints},
motor_commands::MotorCommands,
obstacle_avoiding_arms::ArmCommand,
};

use crate::Context;

pub trait ArmOverrides {
fn override_with_arms(self, cotext: &Context) -> Self;
}

impl ArmOverrides for MotorCommands {
fn override_with_arms(self, context: &Context) -> Self {
let left_swinging_arm = self.positions.left_arm;
let right_swinging_arm = self.positions.right_arm;

    let left_positions =
        compute_joints(&context.obstacle_avoiding_arms.left_arm, left_swinging_arm);
    let right_positions = compute_joints(
        &context.obstacle_avoiding_arms.right_arm,
        right_swinging_arm,
    );

    let positions = BodyJoints {
        left_arm: left_positions,
        right_arm: right_positions,
        left_leg: self.positions.left_leg,
        right_leg: self.positions.right_leg,
    };

    Self { positions, ..self }
}

}

fn compute_joints(arm_command: &ArmCommand, swinging_arms: ArmJoints) -> ArmJoints {
match arm_command {
ArmCommand::Swing => swinging_arms,
ArmCommand::Activating {
influence,
positions,
} => ArmJoints::lerp(*influence, swinging_arms, *positions),
ArmCommand::Active { positions } => *positions,
}
}

the following code is in feet.rs :
use coordinate_systems::{Robot, Walk};
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::Step, support_foot::Side};

use crate::parameters::Parameters;

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Feet {
pub support_sole: Pose3,
pub swing_sole: Pose3,
}

impl Feet {
pub fn from_joints(
robot_to_walk: Isometry3<Robot, Walk>,
joints: &BodyJoints,
support_side: Side,
) -> Self {
let left_sole = robot_to_walk * left_sole_to_robot(&joints.left_leg).as_pose();
let right_sole = robot_to_walk * right_sole_to_robot(&joints.right_leg).as_pose();

    match support_side {
        Side::Left => Feet {
            support_sole: left_sole,
            swing_sole: right_sole,
        },
        Side::Right => Feet {
            support_sole: right_sole,
            swing_sole: left_sole,
        },
    }
}

pub fn end_from_request(parameters: &Parameters, request: Step, support_side: Side) -> Self {
    let (support_base_offset, swing_base_offset) = match support_side {
        Side::Left => (
            parameters.base.foot_offset_left,
            parameters.base.foot_offset_right,
        ),
        Side::Right => (
            parameters.base.foot_offset_right,
            parameters.base.foot_offset_left,
        ),
    };
    let support_sole = Pose3::from_parts(
        point![-request.forward / 2.0, -request.left / 2.0, 0.0] + support_base_offset,
        Orientation3::new(Vector3::z_axis() * -request.turn / 2.0),
    );
    let swing_sole = Pose3::from_parts(
        point![request.forward / 2.0, request.left / 2.0, 0.0] + swing_base_offset,
        Orientation3::new(Vector3::z_axis() * request.turn / 2.0),
    );
    Feet {
        support_sole,
        swing_sole,
    }
}

pub fn swing_travel_over_ground(&self, end: &Feet) -> Vector2<Walk> {
    ((self.support_sole.position() - self.swing_sole.position())
        + (end.swing_sole.position() - end.support_sole.position()))
    .xy()
}

}

the following code is in foot_leveling.rs :
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{joints::body::LowerBodyJoints, support_foot::Side};

use crate::Context;

#[derive(
Debug,
Clone,
Copy,
Serialize,
Deserialize,
Default,
PathSerialize,
PathDeserialize,
PathIntrospect,
)]
pub struct FootLeveling {
pub roll: f32,
pub pitch: f32,
}

impl FootLeveling {
pub fn tick(&mut self, context: &Context, normalized_time_since_start: f32) {
let robot_orientation = *context.robot_orientation;
let robot_to_walk_rotation = context.robot_to_walk.rotation();
let level_orientation = robot_orientation.inner * robot_to_walk_rotation.inner.inverse();
let (level_roll, level_pitch, _) = level_orientation.euler_angles();
let return_factor = ((normalized_time_since_start - 0.5).max(0.0) * 2.0).powi(2);
let target_roll = -level_roll * (1.0 - return_factor);
let target_pitch = -level_pitch * (1.0 - return_factor);

    let max_delta = context.parameters.max_level_delta;

    self.roll = self.roll + (target_roll - self.roll).clamp(-max_delta, max_delta);
    self.pitch = self.pitch + (target_pitch - self.pitch).clamp(-max_delta, max_delta);
}

}

pub trait FootLevelingExt {
fn level_swing_foot(self, state: &FootLeveling, support_side: Side) -> Self;
}

impl FootLevelingExt for LowerBodyJoints {
fn level_swing_foot(mut self, state: &FootLeveling, support_side: Side) -> Self {
let swing_leg = match support_side {
Side::Left => &mut self.right_leg,
Side::Right => &mut self.left_leg,
};
swing_leg.ankle_roll += state.roll;
swing_leg.ankle_pitch += state.pitch;
self
}
}

the following code is in gyro_balancing.rs :
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::{body::LowerBodyJoints, leg::LegJoints},
support_foot::Side,
};

use crate::Context;

#[derive(
Debug,
Clone,
Copy,
Serialize,
Deserialize,
Default,
PathSerialize,
PathDeserialize,
PathIntrospect,
)]
pub struct GyroBalancing {
balancing: LegJoints,
}

impl GyroBalancing {
pub fn tick(&mut self, context: &Context) {
let gyro = context.gyro;
let parameters = &context.parameters.gyro_balancing;
let factors = &parameters.balance_factors;

    let support_balancing = LegJoints {
        ankle_pitch: factors.ankle_pitch * gyro.y,
        ankle_roll: factors.ankle_roll * gyro.x,
        hip_pitch: factors.hip_pitch * gyro.y,
        hip_roll: factors.hip_roll * gyro.x,
        hip_yaw_pitch: 0.0,
        knee_pitch: factors.knee_pitch * gyro.y,
    };

    let max_delta = parameters.max_delta;
    self.balancing =
        self.balancing + (support_balancing - self.balancing).clamp(-max_delta, max_delta);
}

}

pub trait GyroBalancingExt {
fn balance_using_gyro(self, state: &GyroBalancing, support_side: Side) -> Self;
}

impl GyroBalancingExt for LowerBodyJoints {
fn balance_using_gyro(mut self, state: &GyroBalancing, support_side: Side) -> Self {
let support_leg = match support_side {
Side::Left => &mut self.left_leg,
Side::Right => &mut self.right_leg,
};
*support_leg = *support_leg + state.balancing;
self
}
}

the following code is in kick_state.rs :
use itertools::Itertools;
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use splines::Interpolate;
use std::time::Duration;
use types::{
joints::{body::BodyJoints, leg::LegJoints},
motion_command::KickVariant,
support_foot::Side,
};

use crate::kick_steps::{JointOverride, KickStep, KickSteps};

use super::step_state::StepState;

#[derive(
Debug, Copy, Clone, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct KickState {
pub variant: KickVariant,
/// the foot that is kicking the ball
pub side: Side,
pub index: usize,
pub strength: f32,
}

impl KickState {
pub fn new(variant: KickVariant, side: Side, strength: f32) -> Self {
KickState {
variant,
side,
index: 0,
strength,
}
}

pub fn advance_to_next_step(self) -> Self {
    KickState {
        index: self.index + 1,
        ..self
    }
}

pub fn is_finished(&self, kick_steps: &KickSteps) -> bool {
    self.index >= kick_steps.num_steps(self.variant)
}

pub fn get_step<'cycle>(&self, kick_steps: &'cycle KickSteps) -> &'cycle KickStep {
    kick_steps.get_step_at(self.variant, self.index)
}

}

pub trait KickOverride {
fn override_with_kick(self, kick_steps: &KickSteps, kick: &KickState, step: &StepState)
-> Self;
}

impl KickOverride for BodyJoints {
fn override_with_kick(
self,
kick_steps: &KickSteps,
kick: &KickState,
step: &StepState,
) -> Self {
let kick_step = kick_steps.get_step_at(kick.variant, kick.index);
let overrides = compute_kick_overrides(kick_step, step.time_since_start, kick.strength);
match step.plan.support_side {
Side::Left => BodyJoints {
right_leg: self.right_leg + overrides,
..self
},
Side::Right => BodyJoints {
left_leg: self.left_leg + overrides,
..self
},
}
}
}

fn compute_kick_overrides(kick_step: &KickStep, t: Duration, strength: f32) -> LegJoints {
let hip_pitch = kick_step
.hip_pitch_overrides
.as_ref()
.map(|overrides| strength * compute_override(overrides, t))
.unwrap_or(0.0);
let ankle_pitch = kick_step
.ankle_pitch_overrides
.as_ref()
.map(|overrides| strength * compute_override(overrides, t))
.unwrap_or(0.0);
LegJoints {
hip_yaw_pitch: 0.0,
hip_pitch,
hip_roll: 0.0,
knee_pitch: 0.0,
ankle_pitch,
ankle_roll: 0.0,
}
}

fn compute_override(overrides: &[JointOverride], t: Duration) -> f32 {
let Some((start, end)) = overrides
.iter()
.tuple_windows()
.find(|(start, end)| (start.timepoint..end.timepoint).contains(&t))
else {
return 0.0;
};

let phase_duration = end.timepoint - start.timepoint;
let t_in_phase = t - start.timepoint;
let linear_time = (t_in_phase.as_secs_f32() / phase_duration.as_secs_f32()).clamp(0.0, 1.0);
f32::lerp(linear_time, start.value, end.value)

}

the following code is in kick_steps.rs :
use std::time::Duration;

use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{motion_command::KickVariant, step::Step};

#[derive(Clone, Copy, Debug, Serialize, Deserialize)]
pub struct JointOverride {
pub value: f32,
pub timepoint: Duration,
}

#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct KickStep {
pub base_step: Step,
pub step_duration: Duration,
pub foot_lift_apex: f32,
pub midpoint: f32,
pub hip_pitch_overrides: Option<Vec>,
pub ankle_pitch_overrides: Option<Vec>,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct KickSteps {
pub forward: Vec,
pub turn: Vec,
pub side: Vec,
}

impl KickSteps {
pub fn get_steps(&self, variant: KickVariant) -> &[KickStep] {
match variant {
KickVariant::Forward => &self.forward,
KickVariant::Turn => &self.turn,
KickVariant::Side => &self.side,
}
}

pub fn num_steps(&self, variant: KickVariant) -> usize {
    self.get_steps(variant).len()
}

pub fn get_step_at(&self, variant: KickVariant, index: usize) -> &KickStep {
    &self.get_steps(variant)[index]
}

}

the following code is in lib.rs :
use arm::ArmOverrides as _;
use coordinate_systems::{Field, Ground, Robot, Walk};
use kick_steps::KickSteps;
use linear_algebra::{Isometry3, Orientation3, Point2, Point3};
use mode::{
catching::Catching, kicking::Kicking, standing::Standing, starting::Starting,
stopping::Stopping, walking::Walking, Mode,
};
use parameters::Parameters;
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
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::Step, support_foot::Side,
};

mod anatomic_constraints;
mod arm;
pub mod feet;
mod foot_leveling;
mod gyro_balancing;
mod kick_state;
pub mod kick_steps;
pub mod mode;
pub mod parameters;
mod step_plan;
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).
pub struct Context<'a> {
pub parameters: &'a Parameters,
pub kick_steps: &'a KickSteps,
pub cycle_time: &'a CycleTime,
pub center_of_mass: &'a Point3,
pub zero_moment_point: &'a Point2,
pub number_of_consecutive_cycles_zero_moment_point_outside_support_polygon: &'a i32,
pub force_sensitive_resistors: &'a ForceSensitiveResistors,
pub robot_orientation: &'a Orientation3,
pub robot_to_ground: Option<&'a Isometry3<Robot, Ground>>,
pub gyro: nalgebra::Vector3,
pub current_joints: BodyJoints,
pub robot_to_walk: Isometry3<Robot, Walk>,
pub obstacle_avoiding_arms: &'a ArmCommands,
}

pub trait WalkTransition {
fn stand(self, context: &Context) -> Mode;
fn walk(self, context: &Context, request: Step) -> Mode;
fn kick(self, context: &Context, variant: KickVariant, side: Side, strength: f32) -> Mode;
}

#[derive(Debug, Clone, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect)]
pub struct Engine {
pub mode: Mode,
}

impl Default for Engine {
fn default() -> Self {
Self {
mode: Mode::Standing(Standing {}),
}
}
}

impl Engine {
pub fn stand(&mut self, context: &Context) {
self.mode = self.mode.stand(context);
}

pub fn walk(&mut self, context: &Context, request: Step) {
    self.mode = self.mode.walk(context, request);
}

pub fn kick(&mut self, context: &Context, variant: KickVariant, side: Side, strength: f32) {
    self.mode = self.mode.kick(context, variant, side, strength);
}

pub fn tick(&mut self, context: &Context) {
    self.mode.tick(context);
}

pub fn compute_commands(&self, context: &Context) -> MotorCommands<BodyJoints> {
    self.mode
        .compute_commands(context)
        .override_with_arms(context)
}

pub fn is_standing(&self) -> bool {
    matches!(self.mode, Mode::Standing(_))
}

pub fn support_side(&self) -> Option<Side> {
    match self.mode {
        Mode::Standing(_) => None,
        Mode::Starting(Starting { step })
        | Mode::Walking(Walking { step, .. })
        | Mode::Kicking(Kicking { step, .. })
        | Mode::Stopping(Stopping { step, .. })
        | Mode::Catching(Catching { step, .. }) => Some(step.plan.support_side),
    }
}

}

the following code is in mode.rs :
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

use crate::{Context, WalkTransition};

use self::{
catching::Catching, kicking::Kicking, standing::Standing, starting::Starting,
stopping::Stopping, walking::Walking,
};

pub mod catching;
pub mod kicking;
pub mod standing;
pub mod starting;
pub mod stopping;
pub mod walking;

#[derive(
Copy, Clone, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub enum Mode {
Standing(Standing),
Starting(Starting),
Walking(Walking),
Kicking(Kicking),
Stopping(Stopping),
Catching(Catching),
}

impl WalkTransition for Mode {
fn stand(self, context: &Context) -> Mode {
match self {
Self::Standing(standing) => standing.stand(context),
Self::Starting(starting) => starting.stand(context),
Self::Walking(walking) => walking.stand(context),
Self::Kicking(kicking) => kicking.stand(context),
Self::Stopping(stopping) => stopping.stand(context),
Self::Catching(catching) => catching.stand(context),
}
}

fn walk(self, context: &Context, step: Step) -> Mode {
    match self {
        Self::Standing(standing) => standing.walk(context, step),
        Self::Starting(starting) => starting.walk(context, step),
        Self::Walking(walking) => walking.walk(context, step),
        Self::Kicking(kicking) => kicking.walk(context, step),
        Self::Stopping(stopping) => stopping.walk(context, step),
        Self::Catching(catching) => catching.walk(context, step),
    }
}

fn kick(self, context: &Context, variant: KickVariant, side: Side, strength: f32) -> Mode {
    match self {
        Self::Standing(standing) => standing.kick(context, variant, side, strength),
        Self::Starting(starting) => starting.kick(context, variant, side, strength),
        Self::Walking(walking) => walking.kick(context, variant, side, strength),
        Self::Kicking(kicking) => kicking.kick(context, variant, side, strength),
        Self::Stopping(stopping) => stopping.kick(context, variant, side, strength),
        Self::Catching(catching) => catching.kick(context, variant, side, strength),
    }
}

}

impl Mode {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
match self {
Self::Standing(standing) => standing.compute_commands(context),
Self::Starting(starting) => starting.compute_commands(context),
Self::Walking(walking) => walking.compute_commands(context),
Self::Kicking(kicking) => kicking.compute_commands(context),
Self::Stopping(stopping) => stopping.compute_commands(context),
Self::Catching(catching) => catching.compute_commands(context),
}
}

pub fn tick(&mut self, context: &Context) {
    match self {
        Mode::Standing(standing) => standing.tick(context),
        Mode::Starting(starting) => starting.tick(context),
        Mode::Walking(walking) => walking.tick(context),
        Mode::Kicking(kicking) => kicking.tick(context),
        Mode::Stopping(stopping) => stopping.tick(context),
        Mode::Catching(catching) => catching.tick(context),
    }
}

}

the following code is in parameters.rs :
use std::time::Duration;

use coordinate_systems::Walk;
use linear_algebra::Vector3;
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::{arm::ArmJoints, leg::LegJoints},
step::Step,
};

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Parameters {
pub base: Base,
pub catching_steps: CatchingStepsParameters,
pub gyro_balancing: GyroBalancingParameters,
pub max_forward_acceleration: f32,
pub max_inside_turn: f32,
pub max_step_duration: Duration,
pub max_support_foot_lift_speed: f32,
pub min_step_duration: Duration,
pub sole_pressure_threshold: f32,
pub starting_step: StartingStepParameters,
pub step_midpoint: Step,
pub stiffnesses: Stiffnesses,
pub swinging_arms: SwingingArmsParameters,
pub max_level_delta: f32,
pub max_rotation_speed: f32,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Base {
pub foot_lift_apex: f32,
pub foot_lift_apex_increase: Step,
pub foot_offset_left: Vector3,
pub foot_offset_right: Vector3,
pub step_duration: Duration,
pub step_duration_increase: Step,
pub step_midpoint: f32,
pub torso_offset: f32,
pub torso_tilt: f32,
pub walk_height: f32,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct StartingStepParameters {
pub foot_lift_apex: f32,
pub step_duration: Duration,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Stiffnesses {
pub arm_stiffness: f32,
pub leg_stiffness_walk: f32,
pub leg_stiffness_stand: f32,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct GyroBalancingParameters {
pub balance_factors: LegJoints,
pub low_pass_factor: f32,
pub max_delta: LegJoints,
}

#[derive(
Copy,
Clone,
Debug,
Default,
Deserialize,
Serialize,
PathSerialize,
PathDeserialize,
PathIntrospect,
)]
pub struct CatchingStepsParameters {
pub enabled: bool,
pub catching_step_zero_moment_point_frame_count_threshold: i32,
pub max_adjustment: f32,
pub midpoint: f32,
pub target_overestimation_factor: f32,
pub longitudinal_offset: f32,
pub additional_foot_lift: f32,
}

#[derive(
Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct SwingingArmsParameters {
pub default_roll: f32,
pub roll_factor: f32,
pub pitch_factor: f32,
pub pull_back_joints: ArmJoints,
pub pull_tight_joints: ArmJoints,
pub pulling_back_duration: Duration,
pub pulling_tight_duration: Duration,
pub torso_tilt_compensation_factor: f32,
}

the following code is in step_plan.rs :
use std::time::Duration;

use coordinate_systems::Walk;
use linear_algebra::Vector2;
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{step::Step, support_foot::Side};

use crate::Context;

use super::{anatomic_constraints::AnatomicConstraints, feet::Feet};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct StepPlan {
pub step_duration: Duration,
pub start_feet: Feet,
pub end_feet: Feet,
pub support_side: Side,
pub foot_lift_apex: f32,
pub midpoint: f32,
}

impl StepPlan {
pub fn new_from_request(context: &Context, requested_step: Step, support_side: Side) -> Self {
let parameters = &context.parameters;
let start_feet =
Feet::from_joints(context.robot_to_walk, &context.current_joints, support_side);

    let step =
        requested_step.clamp_to_anatomic_constraints(support_side, parameters.max_inside_turn);
    let end_feet = Feet::end_from_request(parameters, step, support_side);

    let swing_foot_travel = start_feet.swing_travel_over_ground(&end_feet).abs();
    let turn_travel = end_feet
        .swing_sole
        .orientation()
        .angle_to(start_feet.swing_sole.orientation());

    let foot_lift_apex = parameters.base.foot_lift_apex
        + travel_weighting(
            swing_foot_travel,
            turn_travel,
            parameters.base.foot_lift_apex_increase,
        );

    let step_duration = parameters.base.step_duration
        + Duration::from_secs_f32(travel_weighting(
            swing_foot_travel,
            turn_travel,
            parameters.base.step_duration_increase,
        ));

    let midpoint = interpolate_midpoint(
        swing_foot_travel,
        parameters.step_midpoint,
        parameters.base.step_midpoint,
    );

    StepPlan {
        step_duration,
        start_feet,
        end_feet,
        support_side,
        foot_lift_apex,
        midpoint,
    }
}

}

fn interpolate_midpoint(
swing_foot_travel: Vector2,
target_midpoints: Step,
base_midpoint: f32,
) -> f32 {
match swing_foot_travel.try_normalize(f32::EPSILON) {
Some(travel) => travel.x() * target_midpoints.forward + travel.y() * target_midpoints.left,
None => base_midpoint,
}
}

fn travel_weighting(translation_travel: Vector2, turn_travel: f32, factors: Step) -> f32 {
let translational = nalgebra::vector![
factors.forward * translation_travel.x(),
factors.left * translation_travel.y(),
]
.norm();
let rotational = factors.turn * turn_travel;
translational + rotational
}

the following code is in step_state.rs :
use std::{f32::consts::FRAC_PI_2, time::Duration};

use coordinate_systems::{LeftSole, RightSole, Walk};
use kinematics::inverse::leg_angles;
use linear_algebra::{point, Isometry3, Orientation3, Point3, Pose3, Rotation3};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use splines::Interpolate;
use types::{
joints::{arm::ArmJoints, body::BodyJoints, leg::LegJoints, mirror::Mirror},
robot_dimensions::RobotDimensions,
support_foot::Side,
};

use crate::{
parameters::{Parameters, SwingingArmsParameters},
Context,
};

use super::{
feet::Feet,
foot_leveling::{FootLeveling, FootLevelingExt},
gyro_balancing::{GyroBalancing, GyroBalancingExt},
step_plan::StepPlan,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct StepState {
pub plan: StepPlan,
pub time_since_start: Duration,
pub gyro_balancing: GyroBalancing,
pub foot_leveling: FootLeveling,
}

impl StepState {
pub fn new(plan: StepPlan) -> Self {
StepState {
plan,
time_since_start: Duration::ZERO,
gyro_balancing: Default::default(),
foot_leveling: Default::default(),
}
}

pub fn tick(&mut self, context: &Context) {
    self.time_since_start += context.cycle_time.last_cycle_duration;
    self.gyro_balancing.tick(context);
    self.foot_leveling
        .tick(context, self.normalized_time_since_start());
}

pub fn is_support_switched(&self, context: &Context) -> bool {
    let pressure_left = context.force_sensitive_resistors.left.sum()
        > context.parameters.sole_pressure_threshold;
    let pressure_right = context.force_sensitive_resistors.right.sum()
        > context.parameters.sole_pressure_threshold;

    let minimal_time = self.time_since_start > context.parameters.min_step_duration;
    let is_support_switched = match self.plan.support_side {
        Side::Left => pressure_right,
        Side::Right => pressure_left,
    };

    minimal_time && is_support_switched
}

pub fn is_timeouted(&self, parameters: &Parameters) -> bool {
    self.time_since_start > parameters.max_step_duration
}

pub fn compute_joints(&self, context: &Context) -> BodyJoints {
    let feet = self.compute_feet(context.parameters);

    let (left_sole, right_sole) = match self.plan.support_side {
        Side::Left => (feet.support_sole, feet.swing_sole),
        Side::Right => (feet.swing_sole, feet.support_sole),
    };
    let walk_to_robot = context.robot_to_walk.inverse();

    let left_foot: Pose3<LeftSole> = Isometry3::from(RobotDimensions::LEFT_ANKLE_TO_LEFT_SOLE)
        .inverse()
        .as_pose();
    let left_sole_to_robot = (walk_to_robot * left_sole).as_transform();
    let right_foot: Pose3<RightSole> =
        Isometry3::from(RobotDimensions::RIGHT_ANKLE_TO_RIGHT_SOLE)
            .inverse()
            .as_pose();
    let right_sole_to_robot = (walk_to_robot * right_sole).as_transform();

    let leg_joints = leg_angles(
        left_sole_to_robot * left_foot,
        right_sole_to_robot * right_foot,
    )
    .balance_using_gyro(&self.gyro_balancing, self.plan.support_side)
    .level_swing_foot(&self.foot_leveling, self.plan.support_side);

    let left_arm = swinging_arm(
        &context.parameters.swinging_arms,
        leg_joints.left_leg,
        right_sole.position().x(),
    );
    let right_arm = swinging_arm(
        &context.parameters.swinging_arms,
        leg_joints.right_leg.mirrored(),
        left_sole.position().x(),
    )
    .mirrored();

    BodyJoints {
        left_arm,
        right_arm,
        left_leg: leg_joints.left_leg,
        right_leg: leg_joints.right_leg,
    }
}

fn normalized_time_since_start(&self) -> f32 {
    (self.time_since_start.as_secs_f32() / self.plan.step_duration.as_secs_f32())
        .clamp(0.0, 1.0)
}

fn compute_feet(&self, parameters: &Parameters) -> Feet {
    let support_sole = self.support_sole_position(parameters);
    let support_turn = self.support_orientation(parameters);
    let swing_sole = self.swing_sole_position();
    let swing_turn = self.swing_orientation(parameters);

    let support_sole = Pose3::from_parts(support_sole, support_turn);
    let swing_sole = Pose3::from_parts(swing_sole, swing_turn);

    Feet {
        support_sole,
        swing_sole,
    }
}

fn support_sole_position(&self, parameters: &Parameters) -> Point3<Walk> {
    let normalized_time = self.normalized_time_since_start();

    let start_offsets = self.plan.start_feet.support_sole.position().xy();
    let end_offsets = self.plan.end_feet.support_sole.position().xy();
    let offsets = start_offsets.lerp(end_offsets, normalized_time);

    let lift = self.support_sole_lift_at(parameters);

    point![offsets.x(), offsets.y(), lift]
}

fn support_sole_lift_at(&self, parameters: &Parameters) -> f32 {
    let start_lift = self.plan.start_feet.support_sole.position().z();
    let end_lift = self.plan.end_feet.support_sole.position().z();

    let max_lift_speed = parameters.max_support_foot_lift_speed;
    let max_lift_delta = self.time_since_start.as_secs_f32() * max_lift_speed;

    start_lift + (end_lift - start_lift).clamp(-max_lift_delta, max_lift_delta)
}

fn support_orientation(&self, parameters: &Parameters) -> Orientation3<Walk> {
    let normalized_time = self.normalized_time_since_start();
    let start = self.plan.start_feet.support_sole.orientation();
    let target = self.plan.end_feet.support_sole.orientation();

    let max_rotation_delta =
        self.time_since_start.as_secs_f32() * parameters.max_rotation_speed;

    let (roll, pitch, yaw) = start.rotation_to(target).inner.euler_angles();
    let interpolated_roll = roll.clamp(-max_rotation_delta, max_rotation_delta);
    let interpolated_pitch = pitch.clamp(-max_rotation_delta, max_rotation_delta);
    let interpolated_yaw = f32::lerp(normalized_time, 0.0, yaw);
    let interpolated =
        Rotation3::from_euler_angles(interpolated_roll, interpolated_pitch, interpolated_yaw);

    interpolated * start
}

fn swing_sole_position(&self) -> Point3<Walk> {
    let normalized_time = self.normalized_time_since_start();
    let parabolic_time = parabolic_step(normalized_time);

    let start_offsets = self.plan.start_feet.swing_sole.position().xy();
    let end_offsets = self.plan.end_feet.swing_sole.position().xy();

    let offsets = start_offsets.lerp(end_offsets, parabolic_time);
    let lift = self.swing_sole_lift_at();

    point![offsets.x(), offsets.y(), lift]
}

fn swing_sole_lift_at(&self) -> f32 {
    let normalized_time = self.normalized_time_since_start();
    let parabolic_time = parabolic_return(normalized_time, self.plan.midpoint);

    let start_lift = self.plan.start_feet.swing_sole.position().z();
    let end_lift = self.plan.end_feet.swing_sole.position().z();

    let linear_lift = f32::lerp(normalized_time, start_lift, end_lift);
    let parabolic_lift = self.plan.foot_lift_apex * parabolic_time;

    parabolic_lift + linear_lift
}

fn swing_orientation(&self, parameters: &Parameters) -> Orientation3<Walk> {
    let normalized_time = self.normalized_time_since_start();
    let start = self.plan.start_feet.swing_sole.orientation();
    let target = self.plan.end_feet.swing_sole.orientation();

    let max_rotation_speed = parameters.max_rotation_speed;
    let max_rotation_delta = self.time_since_start.as_secs_f32() * max_rotation_speed;

    let (roll, pitch, yaw) = start.rotation_to(target).inner.euler_angles();
    let interpolated_roll = roll.clamp(-max_rotation_delta, max_rotation_delta);
    let interpolated_pitch = pitch.clamp(-max_rotation_delta, max_rotation_delta);
    let interpolated_yaw = f32::lerp(normalized_time, 0.0, yaw);
    let interpolated =
        Rotation3::from_euler_angles(interpolated_roll, interpolated_pitch, interpolated_yaw);

    interpolated * start
}

}

fn swinging_arm(
parameters: &SwingingArmsParameters,
same_leg: LegJoints,
opposite_foot_x: f32,
) -> ArmJoints {
let shoulder_roll = parameters.default_roll + parameters.roll_factor * same_leg.hip_roll;
let shoulder_pitch = FRAC_PI_2 - opposite_foot_x * parameters.pitch_factor;
ArmJoints {
shoulder_pitch,
shoulder_roll,
elbow_yaw: 0.0,
elbow_roll: 0.0,
wrist_yaw: -FRAC_PI_2,
hand: 0.0,
}
}

// visualized in desmos: https://www.desmos.com/calculator/kcr3uxqmyw
fn parabolic_return(x: f32, midpoint: f32) -> f32 {
if x < midpoint {
-2.0 / midpoint.powi(3) * x.powi(3) + 3.0 / midpoint.powi(2) * x.powi(2)
} else {
-1.0 / (midpoint - 1.0).powi(3)
* (2.0 * x.powi(3) - 3.0 * (midpoint + 1.0) * x.powi(2) + 6.0 * midpoint * x
- 3.0 * midpoint
+ 1.0)
}
}

fn parabolic_step(x: f32) -> f32 {
if x < 0.5 {
2.0 * x * x
} else {
4.0 * x - 2.0 * x * x - 1.0
}
}

the following code is in stiffness.rs :
use types::{
joints::body::{BodyJoints, LowerBodyJoints, UpperBodyJoints},
motor_commands::MotorCommands,
};

pub trait Stiffness {
fn apply_stiffness(self, legs: f32, arms: f32) -> MotorCommands<BodyJoints>;
}

impl Stiffness for BodyJoints {
fn apply_stiffness(self, legs: f32, arms: f32) -> MotorCommands<BodyJoints> {
let stiffnesses = BodyJoints::from_lower_and_upper(
LowerBodyJoints::fill(legs),
UpperBodyJoints::fill(arms),
);
MotorCommands {
positions: self,
stiffnesses,
}
}
}

the following code is in catching.rs :
use std::time::Duration;

use crate::{
anatomic_constraints::AnatomicConstraints, parameters::Parameters, step_plan::StepPlan,
stiffness::Stiffness as _, Context,
};

use super::{
super::{feet::Feet, step_state::StepState},
stopping::Stopping,
walking::Walking,
Mode, WalkTransition,
};
use coordinate_systems::Ground;
use linear_algebra::Point2;
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Catching {
pub step: StepState,
}

impl Catching {
pub fn new(context: &Context, support_side: Side) -> Self {
let parameters = &context.parameters;

    let step_duration = parameters.base.step_duration;
    let start_feet =
        Feet::from_joints(context.robot_to_walk, &context.current_joints, support_side);

    let end_feet = catching_end_feet(parameters, *context.zero_moment_point, support_side);
    let max_swing_foot_lift =
        parameters.base.foot_lift_apex + parameters.catching_steps.additional_foot_lift;
    let midpoint = parameters.catching_steps.midpoint;

    let step = StepState {
        plan: StepPlan {
            step_duration,
            start_feet,
            end_feet,
            support_side,
            foot_lift_apex: max_swing_foot_lift,
            midpoint,
        },
        time_since_start: Duration::ZERO,
        gyro_balancing: Default::default(),
        foot_leveling: Default::default(),
    };
    Self { step }
}

fn next_step(self, context: &Context) -> Mode {
    let current_step = self.step;

    if context.robot_to_ground.is_none() {
        return Mode::Stopping(Stopping::new(context, current_step.plan.support_side));
    }
    if *context.number_of_consecutive_cycles_zero_moment_point_outside_support_polygon
        <= context
            .parameters
            .catching_steps
            .catching_step_zero_moment_point_frame_count_threshold
    {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }
    Mode::Catching(self)
}

}

fn catching_end_feet(
parameters: &Parameters,
zero_moment_point: Point2,
support_side: Side,
) -> Feet {
let target_overestimation_factor = parameters.catching_steps.target_overestimation_factor;
let longitudinal_zero_moment_point_offset = parameters.catching_steps.longitudinal_offset;
let max_adjustment = parameters.catching_steps.max_adjustment;

Feet::end_from_request(
    parameters,
    Step {
        forward: ((zero_moment_point.x() + longitudinal_zero_moment_point_offset)
            * target_overestimation_factor)
            .clamp(-max_adjustment, max_adjustment),
        left: (zero_moment_point.y() * target_overestimation_factor)
            .clamp(-max_adjustment, max_adjustment),
        turn: 0.0,
    }
    .clamp_to_anatomic_constraints(support_side, parameters.max_inside_turn),
    support_side,
)

}

impl WalkTransition for Catching {
fn stand(self, context: &Context) -> Mode {
let current_step = self.step;
if current_step.is_support_switched(context) {
return self.next_step(context);
}

    Mode::Catching(self)
}

fn walk(self, context: &Context, _requested_step: Step) -> Mode {
    let current_step = self.step;
    if current_step.is_support_switched(context) {
        return self.next_step(context);
    }

    Mode::Catching(self)
}

fn kick(self, context: &Context, _variant: KickVariant, _side: Side, _strength: f32) -> Mode {
    let current_step = self.step;
    if current_step.is_support_switched(context) {
        return self.next_step(context);
    }

    Mode::Catching(self)
}

}

impl Catching {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
self.step.compute_joints(context).apply_stiffness(
context.parameters.stiffnesses.leg_stiffness_walk,
context.parameters.stiffnesses.arm_stiffness,
)
}

pub fn tick(&mut self, context: &Context) {
    self.step.plan.end_feet = catching_end_feet(
        context.parameters,
        *context.zero_moment_point,
        self.step.plan.support_side,
    );
    self.step.tick(context);
}

}

the following code is in kicking.rs :
use std::time::Duration;

use crate::{
feet::Feet,
kick_state::{KickOverride as _, KickState},
step_plan::StepPlan,
step_state::StepState,
stiffness::Stiffness as _,
Context,
};

use super::{stopping::Stopping, walking::Walking, Mode, WalkTransition};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Kicking {
pub kick: KickState,
pub step: StepState,
}

impl Kicking {
pub fn new(context: &Context, kick: KickState, support_side: Side) -> Self {
let start_feet =
Feet::from_joints(context.robot_to_walk, &context.current_joints, support_side);

    let kick_step = kick.get_step(context.kick_steps);
    let base_step = kick_step.base_step;
    let request = match kick.side {
        Side::Left => base_step,
        Side::Right => base_step.mirrored(),
    };
    let end_feet = Feet::end_from_request(context.parameters, request, support_side);

    let step = StepState {
        plan: StepPlan {
            step_duration: kick_step.step_duration,
            start_feet,
            end_feet,
            support_side,
            foot_lift_apex: kick_step.foot_lift_apex,
            midpoint: kick_step.midpoint,
        },
        time_since_start: Duration::ZERO,
        gyro_balancing: Default::default(),
        foot_leveling: Default::default(),
    };
    Self { kick, step }
}

}

impl WalkTransition for Kicking {
fn stand(self, context: &Context) -> Mode {
let current_step = self.step;
if current_step.is_support_switched(context)
|| current_step.is_timeouted(context.parameters)
{
return Mode::Stopping(Stopping::new(
context,
current_step.plan.support_side.opposite(),
));
}

    Mode::Kicking(self)
}

fn walk(self, context: &Context, step: Step) -> Mode {
    let current_step = self.step;
    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    if current_step.is_support_switched(context) {
        let kick = self.kick.advance_to_next_step();
        if kick.is_finished(context.kick_steps) {
            return Mode::Walking(Walking::new(
                context,
                step,
                current_step.plan.support_side.opposite(),
                Step::ZERO,
            ));
        }

        return Mode::Kicking(Kicking::new(
            context,
            kick,
            current_step.plan.support_side.opposite(),
        ));
    }

    Mode::Kicking(self)
}

fn kick(
    self,
    context: &Context,
    variant: KickVariant,
    kicking_side: Side,
    strength: f32,
) -> Mode {
    let current_step = self.step;
    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    if current_step.is_support_switched(context) {
        let next_support_side = current_step.plan.support_side.opposite();
        let current_kick = self.kick.advance_to_next_step();
        if !current_kick.is_finished(context.kick_steps) {
            return Mode::Kicking(Kicking::new(context, current_kick, next_support_side));
        }

        // TODO: all kicks require a pre-step
        if next_support_side != kicking_side {
            return Mode::Walking(Walking::new(
                context,
                Step::ZERO,
                next_support_side,
                Step::ZERO,
            ));
        }
        return Mode::Kicking(Kicking::new(
            context,
            KickState::new(variant, kicking_side, strength),
            next_support_side,
        ));
    }

    Mode::Kicking(self)
}

}

impl Kicking {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
self.step
.compute_joints(context)
.override_with_kick(context.kick_steps, &self.kick, &self.step)
.apply_stiffness(
context.parameters.stiffnesses.leg_stiffness_walk,
context.parameters.stiffnesses.arm_stiffness,
)
}

pub fn tick(&mut self, context: &Context) {
    self.step.tick(context);
}

}

the following code is in standing.rs :
use std::time::Duration;

use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

use crate::{
feet::Feet, step_plan::StepPlan, step_state::StepState, stiffness::Stiffness as _, Context,
WalkTransition,
};

use super::{starting::Starting, Mode};

#[derive(
Default,
Clone,
Copy,
Debug,
Serialize,
Deserialize,
PathSerialize,
PathDeserialize,
PathIntrospect,
)]
pub struct Standing {}

impl WalkTransition for Standing {
fn stand(self, _context: &Context) -> Mode {
Mode::Standing(Standing {})
}

fn walk(self, context: &Context, step: Step) -> Mode {
    let is_requested_step_towards_left = step.left.is_sign_positive();
    let support_side = if is_requested_step_towards_left {
        Side::Left
    } else {
        Side::Right
    };
    Mode::Starting(Starting::new(context, support_side))
}

fn kick(
    self,
    context: &Context,
    _variant: KickVariant,
    kicking_side: Side,
    _strength: f32,
) -> Mode {
    let support_side = if kicking_side == Side::Left {
        Side::Left
    } else {
        Side::Right
    };
    Mode::Starting(Starting::new(context, support_side))
}

}

impl Standing {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
let feet = Feet::end_from_request(context.parameters, Step::ZERO, Side::Left);

    let zero_step_state = StepState {
        plan: StepPlan {
            step_duration: Duration::from_secs(1),
            start_feet: feet,
            end_feet: feet,
            support_side: Side::Left,
            foot_lift_apex: 0.0,
            midpoint: 0.5,
        },
        time_since_start: Duration::ZERO,
        gyro_balancing: Default::default(),
        foot_leveling: Default::default(),
    };
    zero_step_state.compute_joints(context).apply_stiffness(
        context.parameters.stiffnesses.leg_stiffness_stand,
        context.parameters.stiffnesses.arm_stiffness,
    )
}

pub fn tick(&mut self, _context: &Context) {}

}

the following code is in starting.rs :
use crate::{
kick_state::KickState, step_plan::StepPlan, step_state::StepState, stiffness::Stiffness as _,
Context,
};

use super::{kicking::Kicking, standing::Standing, Mode, WalkTransition, Walking};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Starting {
pub step: StepState,
}

impl Starting {
pub fn new(context: &Context, support_side: Side) -> Self {
let plan = StepPlan::new_from_request(context, Step::ZERO, support_side);
let step = StepState::new(plan);
Self { step }
}
}

impl WalkTransition for Starting {
fn stand(self, context: &Context) -> Mode {
let current_step = self.step;
if current_step.is_support_switched(context)
|| current_step.is_timeouted(context.parameters)
{
return Mode::Standing(Standing {});
}

    Mode::Starting(self)
}

fn walk(self, context: &Context, requested_step: Step) -> Mode {
    let current_step = self.step;

    if current_step.is_support_switched(context)
        || current_step.is_timeouted(context.parameters)
    {
        return Mode::Walking(Walking::new(
            context,
            requested_step,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    Mode::Starting(self)
}

fn kick(self, context: &Context, variant: KickVariant, kick_side: Side, strength: f32) -> Mode {
    let current_step = self.step;

    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    if current_step.is_support_switched(context) {
        let next_support_side = current_step.plan.support_side.opposite();
        if next_support_side == kick_side {
            return Mode::Walking(Walking::new(
                context,
                Step::ZERO,
                current_step.plan.support_side.opposite(),
                Step::ZERO,
            ));
        }

        let kick = KickState::new(variant, kick_side, strength);
        return Mode::Kicking(Kicking::new(
            context,
            kick,
            current_step.plan.support_side.opposite(),
        ));
    }
    Mode::Starting(self)
}

}

impl Starting {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
self.step.compute_joints(context).apply_stiffness(
context.parameters.stiffnesses.leg_stiffness_walk,
context.parameters.stiffnesses.arm_stiffness,
)
}

pub fn tick(&mut self, context: &Context) {
    self.step.tick(context);
}

}

the following code is in stopping.rs :
use crate::{
kick_state::KickState, step_plan::StepPlan, step_state::StepState, stiffness::Stiffness as _,
Context,
};

use super::{kicking::Kicking, standing::Standing, walking::Walking, Mode, WalkTransition};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Stopping {
pub step: StepState,
}

impl Stopping {
pub fn new(context: &Context, support_side: Side) -> Self {
let plan = StepPlan::new_from_request(context, Step::ZERO, support_side);
let step = StepState::new(plan);
Self { step }
}
}

impl WalkTransition for Stopping {
fn stand(self, context: &Context) -> Mode {
let current_step = self.step;
if current_step.is_support_switched(context)
|| current_step.is_timeouted(context.parameters)
{
return Mode::Standing(Standing {});
}

    Mode::Stopping(self)
}

fn walk(self, context: &Context, requested_step: Step) -> Mode {
    let current_step = self.step;

    if current_step.is_support_switched(context)
        || current_step.is_timeouted(context.parameters)
    {
        return Mode::Walking(Walking::new(
            context,
            requested_step,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    Mode::Stopping(self)
}

fn kick(self, context: &Context, variant: KickVariant, side: Side, strength: f32) -> Mode {
    let current_step = self.step;

    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            Step::ZERO,
        ));
    }

    if current_step.is_support_switched(context) {
        let support_side = current_step.plan.support_side.opposite();
        if support_side == side {
            return Mode::Walking(Walking::new(
                context,
                Step::ZERO,
                current_step.plan.support_side.opposite(),
                Step::ZERO,
            ));
        }

        let kick = KickState::new(variant, side, strength);
        return Mode::Kicking(Kicking::new(
            context,
            kick,
            current_step.plan.support_side.opposite(),
        ));
    }

    Mode::Stopping(self)
}

}

impl Stopping {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
self.step.compute_joints(context).apply_stiffness(
context.parameters.stiffnesses.leg_stiffness_walk,
context.parameters.stiffnesses.arm_stiffness,
)
}

pub fn tick(&mut self, context: &Context) {
    self.step.tick(context);
}

}

the following code is in walking.rs :
use super::{catching::Catching, kicking::Kicking, stopping::Stopping, Mode, WalkTransition};
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
use serde::{Deserialize, Serialize};
use types::{
joints::body::BodyJoints, motion_command::KickVariant, motor_commands::MotorCommands,
step::Step, support_foot::Side,
};

use crate::{
kick_state::KickState, step_plan::StepPlan, step_state::StepState, stiffness::Stiffness as _,
Context,
};

#[derive(
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
)]
pub struct Walking {
pub step: StepState,
pub requested_step: Step,
}

impl Walking {
pub fn new(
context: &Context,
requested_step: Step,
support_side: Side,
last_requested_step: Step,
) -> Self {
let requested_step = Step {
forward: last_requested_step.forward
+ (requested_step.forward - last_requested_step.forward)
.min(context.parameters.max_forward_acceleration),
left: requested_step.left,
turn: requested_step.turn,
};
let plan = StepPlan::new_from_request(context, requested_step, support_side);
let step = StepState::new(plan);
Self {
step,
requested_step,
}
}
}

impl WalkTransition for Walking {
fn stand(self, context: &Context) -> Mode {
let current_step = self.step;
if current_step.is_support_switched(context)
|| current_step.is_timeouted(context.parameters)
{
return Mode::Stopping(Stopping::new(
context,
current_step.plan.support_side.opposite(),
));
}

    Mode::Walking(self)
}

fn walk(self, context: &Context, requested_step: Step) -> Mode {
    let current_step = self.step;

    if context.parameters.catching_steps.enabled {
        if context.robot_to_ground.is_none() {
            return Mode::Stopping(Stopping::new(context, current_step.plan.support_side));
        }

        if *context.number_of_consecutive_cycles_zero_moment_point_outside_support_polygon
            > context
                .parameters
                .catching_steps
                .catching_step_zero_moment_point_frame_count_threshold
        {
            return Mode::Catching(Catching::new(context, current_step.plan.support_side));
        };
    }
    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            self.requested_step,
        ));
    }

    if current_step.is_support_switched(context) {
        return Mode::Walking(Walking::new(
            context,
            requested_step,
            current_step.plan.support_side.opposite(),
            self.requested_step,
        ));
    }

    Mode::Walking(self)
}

fn kick(
    self,
    context: &Context,
    variant: KickVariant,
    kicking_side: Side,
    strength: f32,
) -> Mode {
    let current_step = self.step;

    if current_step.is_timeouted(context.parameters) {
        return Mode::Walking(Walking::new(
            context,
            Step::ZERO,
            current_step.plan.support_side.opposite(),
            self.requested_step,
        ));
    }

    if current_step.is_support_switched(context) {
        let next_support_side = current_step.plan.support_side.opposite();
        // TODO: all kicks require a pre-step
        if next_support_side != kicking_side {
            return Mode::Walking(Walking::new(
                context,
                Step::ZERO,
                next_support_side,
                self.requested_step,
            ));
        }

        return Mode::Kicking(Kicking::new(
            context,
            KickState::new(variant, kicking_side, strength),
            next_support_side,
        ));
    }

    Mode::Walking(self)
}

}

impl Walking {
pub fn compute_commands(&self, context: &Context) -> MotorCommands {
self.step.compute_joints(context).apply_stiffness(
context.parameters.stiffnesses.leg_stiffness_walk,
context.parameters.stiffnesses.arm_stiffness,
)
}

pub fn tick(&mut self, context: &Context) {
    self.step.tick(context);
}

}

[CodeByDamianK]

In the attachment is a pdf paper with the finished documentation and a flowchart of the walking engine.
Down below, is the corresponding Latex code.

\documentclass[a4paper,11pt]{article}
\usepackage{geometry}
\geometry{a4paper, margin=1in}
\usepackage{hyperref}
\usepackage{enumitem}
\usepackage{listings}
\usepackage{xcolor}
\usepackage{float}
\usepackage{ulem}

\usepackage[utf8]{inputenc}
\usepackage[T1]{fontenc}
\usepackage{lmodern}
\usepackage{tikz}
\usetikzlibrary{shapes, arrows.meta, positioning}

\tikzstyle{module} = [rectangle, minimum width=3.5cm, minimum height=1cm, text centered, draw=black, fill=blue!30]
\tikzstyle{submodule} = [rectangle, minimum width=2.5cm, text centered, draw=black, fill=cyan!30]
\tikzstyle{arrow} = [thick,->,>=stealth]
\tikzstyle{dashedarrow} = [thick,dashed,->,>=stealth]

% Define colors for listings
\definecolor{backcolor}{rgb}{0.95,0.95,0.92}
\definecolor{keywordcolor}{rgb}{0.5,0,0.5}
\definecolor{commentcolor}{rgb}{0.0,0.5,0.0}
\definecolor{stringcolor}{rgb}{0.6,0,0}
\definecolor{numbercolor}{rgb}{0.5,0.5,0.5}

% Define language and style for Rust code
\lstdefinelanguage{Rust}{%
morekeywords={as, break, const, continue, crate, else, enum, extern, false, fn, for, if, impl, in, let,
loop, match, mod, move, mut, pub, ref, return, self, Self, static, struct, super, trait, true,
type, unsafe, use, where, while, async, await, dyn,},
sensitive=true,
morecomment=[l]{//},
morecomment=[s]{/}{/},
morestring=[b]",
morestring=[b]',
}

\lstset{%
language=Rust,
backgroundcolor=\color{backcolor},
commentstyle=\color{commentcolor},
keywordstyle=\color{keywordcolor}\bfseries,
stringstyle=\color{stringcolor},
numberstyle=\color{numbercolor},
basicstyle=\ttfamily\small,
breaklines=true,
numbersep=5pt,
frame=single,
showstringspaces=false,
tabsize=2,
}

\title{HULKS Walking Engine Documentation and Flowchart}
\author{}
\date{05.12.24}

\begin{document}

\maketitle

\tableofcontents
\newpage

\section{Introduction}
The robot walking engine is designed to simulate natural and efficient robotic locomotion, encompassing a variety of movements including walking, standing, arm movements, and kicking. This document provides a comprehensive analysis of the codebase's architecture, detailing each module and submodule's functionality, the intricate interactions between components, and the underlying design rationale.

With an increasing demand for robots capable of dynamic and adaptive interactions with their environment, this engine seeks to blend computational efficiency with biomechanical innovation.

\section{Modules Overview}
The walking engine's architecture is divided into a series of core modules, each addressing specific functionalities:

\begin{itemize}
\item \textbf{Anatomic Constraints}: Establishes movement limits based on anatomic constraints of the robot to ensure realistic and stable behaviors.
\item \textbf{Arm}: Manages the calculated movements of arms to mimic human-like motions and handle obstacles.
\item \textbf{Feet}: Controls precise foot positioning and trajectory transformations.
\item \textbf{Foot Leveling}: Maintains foot orientation for balance relative to the terrain.
\item \textbf{Gyro Balancing}: Utilizes gyro input to fine-tune posture and stability.
\item \textbf{Kick State}: Manages the current progression through a kicking action.
\item \textbf{Kick Steps}: Prescribes sequences and adjustments for executing kicks.
\item \textbf{Modes}: Oversees the active state or activity of the robot (e.g., walking, standing).
\item \textbf{Parameters}: Houses configuration settings impacting system behavior.
\item \textbf{Step Plan}: Outlines the step sequences and trajectory plans for walking.
\item \textbf{Step State}: Monitors and updates the execution state of movements.
\item \textbf{Stiffness}: Adjusts the rigidity of joints based on current activities.
\end{itemize}

\newpage

\section{Module Descriptions and Interactions}

\subsection{Anatomic Constraints Module}
\textbf{File:} \texttt{anatomic_constraints.rs}

\textbf{Purpose:} The Anatomic Constraints module enforces limits on the robot's movements to simulate realistic human anatomical constraints. This consideration is crucial in preventing unnatural posture and movements that could lead to mechanical stress or imbalance.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Functionality:} Clamps step movements to adhere to constraints based on the support side and turn limitations. Acts as a safeguard to ensure the step parameters remain within feasible ranges.
\item \textbf{Signature:}
\begin{lstlisting}
fn clamp_to_anatomic_constraints(
self, support_side: Side, maximum_inside_turn: f32
) -> Step;
\end{lstlisting}
\end{itemize}

\subsubsection*{Design Decisions:}
The decision to implement anatomical constraints via a trait allows for greater flexibility and reusability across different motion planning components. By decoupling this logic, the system enhances its adaptability to different robotic platforms with varying mechanical capabilities.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Step Plan Integration:} This module plays a crucial in ensuring that the steps planned by the \texttt{Step Plan} module are within realistic boundaries.
\item \textbf{Scenario Example:} When transitioning from walking to standing, the component ensures balance is maintained by adapting turns and foot placement as needed.
\end{itemize}

\subsection{Arm Module}
\textbf{File:} \texttt{arm.rs}

\textbf{Purpose:} This module orchestrates arm movements to provide naturalistic human-like swinging motions while adapting to potential obstacles. Arm movements also contribute to balance during dynamic actions such as walking.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Functionality:} Takes the current motor command configuration and adjusts the arm positions according to contextual inputs such as obstacle proximity or required swing amplitude.
\item \textbf{Signature:}
\begin{lstlisting}
fn override_with_arms(self, context: &Context) -> Self;
\end{lstlisting}
\item \textbf{Implementation Detail:}
This involves complex interpolation between joint positions, often utilizing physics-based simulations to achieve smooth, efficient motions.
\end{itemize}

\subsubsection*{Design Decisions:}
Opting for a composable approach with the override mechanism empowers real-time adaptability without needing to process new configurations from scratch. This method leverages existing command sets to optimize response time and computational load.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Integration with MotorCommands:} This module modifies the existing \texttt{MotorCommands} which are shared across several movement modules.
\item \textbf{Feedback Loop with Context:} Continuously adapts to environmental changes, ensuring a real-time update mechanism that considers changing terrain or obstacles.
\end{itemize}

\textbf{Data Flow:}
Receives input concerning environmental data and motor command configurations, applies transformations, and reissues commands with arm position adaptations.

\subsection{Feet Module}
\textbf{File:} \texttt{feet.rs}

\textbf{Purpose:} The Feet module is crucial for managing the positioning and trajectory of the robot's feet. It ensures proper placement and movement within various coordinate systems, critical for achieving stable and efficient gaits.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{Feet}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{support_sole: Pose3}: Represents the position and orientation of the supporting foot.
\item \texttt{swing_sole: Pose3}: Represents the position and orientation of the swinging foot.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{from_joints}}: Constructs a \texttt{Feet} instance using joint configurations and transformation matrices.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn from_joints(
robot_to_walk: Isometry3<Robot, Walk>,
joints: &BodyJoints,
support_side: Side
) -> Self;
\end{lstlisting}

\begin{itemize}
\item \textbf{\texttt{end_from_request}}: Computes the final position of the feet based on a step request.
\end{itemize}

\begin{lstlisting}
pub fn end_from_request(parameters: &Parameters, request: Step, support_side: Side) -> Self;
\end{lstlisting}

\subsubsection*{Design Decisions:}
The modular design of the \texttt{Feet} struct allows for flexible handling of different walking scenarios by abstracting foot positioning calculations into easily manageable methods. This decision facilitates straightforward integration with other modules, such as \texttt{Step Plan}.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Integration with \texttt{Step Plan}}: The \texttt{Feet} module provides essential calculations for the \texttt{Step Plan} module, enabling accurate step placement and trajectory planning.
\item \textbf{Interaction with Sensors}: Uses input from sensors to adjust positions dynamically, ensuring the robot maintains balance over varied terrain.
\end{itemize}

\subsection{Foot Leveling Module}
\textbf{File:} \texttt{foot_leveling.rs}

\textbf{Purpose:} The Foot Leveling module adjusts the orientation of the robot's feet in real-time to ensure stability, particularly when navigating uneven terrain or responding to balance perturbations.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{FootLeveling}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{roll: f32}: Represents the roll adjustment applied.
\item \texttt{pitch: f32}: Represents the pitch adjustment applied.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{tick}}: Updates the state of foot leveling based on time and context, applying necessary corrections to the roll and pitch.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn tick(&mut self, context: &Context, normalized_time_since_start: f32);
\end{lstlisting}

\subsubsection*{Design Decisions:}
By abstracting foot leveling into its module, the system can maintain flexibility and adapt to different terrain types without altering core walking logic. This design decision ensures robustness and scalability.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Collaboration with \texttt{Gyro Balancing}}: This module works closely with \texttt{Gyro Balancing} to fine-tune postural stability, providing combined feedback for optimal joint adjustments.
\item \textbf{Feedback from Environmental Analysis}: Continuously assesses sensor data to execute micro-adjustments and review terrain consistency for stable navigation.
\end{itemize}

\textbf{Data Flow:}
The module receives data from the context, including sensor readings and environmental conditions. This data allows the module to calculate and apply needed changes to the foot orientation, ensuring continuous balance maintenance.

\subsection{Gyro Balancing Module}
\textbf{File:} \texttt{gyro_balancing.rs}

\textbf{Purpose:} The Gyro Balancing module uses gyroscope data to dynamically adjust the robot's posture, maintaining balance and stability during movement. This real-time adjustment is crucial for adapting to unexpected shifts in the robot's center of gravity.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{GyroBalancing}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{balancing: LegJoints}: Represents the calculated adjustments to apply to the leg joints.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{tick}}: Processes gyro data and updates the balancing adjustments.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn tick(&mut self, context: &Context);
\end{lstlisting}

\subsubsection*{Design Decisions:}
The separation of gyro balancing logic into its module allows for dedicated processing of sensor data, which enhances the robot's ability to respond quickly to balance shifts. This modularity supports extensibility for incorporating additional stabilization sensors.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Integration with \texttt{Step State}}: This module supplies the changes needed to maintain balance, which are applied during each step cycle managed by \texttt{Step State}.
\item \textbf{Collaboration with \texttt{Foot Leveling}}: Ensures that jointly they provide a comprehensive stabilization strategy.
\end{itemize}

\textbf{Data Flow:}
Gyro readings are continuously processed, leading to real-time modifications of leg joint commands, thereby maintaining the desired orientation and balance.

\subsection{Kick State Module}
\textbf{File:} \texttt{kick_state.rs}

\textbf{Purpose:} The Kick State module tracks and controls the execution of kicking maneuvers. It manages the state of the kick process and transitions between different kick steps, ensuring that each phase of the kick is executed accurately.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{KickState}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{variant: KickVariant}: Specifies the type of kick being executed.
\item \texttt{side: Side}: Indicates which foot is performing the kick.
\item \texttt{index: usize}: Tracks the current step in the kick sequence.
\item \texttt{strength: f32}: Represents the force applied in the kick.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{advance_to_next_step}}: Progresses to the next step in the kick sequence.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn advance_to_next_step(self) -> Self;
pub fn is_finished(&self, kick_steps: &KickSteps) -> bool;
\end{lstlisting}

\subsubsection*{Design Decisions:}
By encapsulating kick progression logic within a single module, the system maintains clarity and separation of concerns. This design makes it easier to modify or expand kick behaviors without disrupting other movement systems.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Interaction with \texttt{Kick Steps}}: Utilizes predefined sequences from the \texttt{Kick Steps} module to execute complex kicks effectively.
\item \textbf{Integration with \texttt{Modes}}: Activated and managed within different operational modes, enabling smooth transitions between walking and kicking.
\end{itemize}

\textbf{Data Flow:}
The module manages the progression of kick steps, ensuring seamless data handoff with the \texttt{Kick Steps} module to dictate movement timings and execution.

\subsection{Kick Steps Module}
\textbf{File:} \texttt{kick_steps.rs}

\textbf{Purpose:} The Kick Steps module defines and manages the sequences of steps and joint overrides required to perform kicking actions. Each type of kick is preconfigured with specific movements to ensure precision and effectiveness.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{KickSteps}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{forward: Vec}: Represents a sequence of steps for a forward kick.
\item \texttt{turn: Vec}: Represents a sequence of steps for a turning kick.
\item \texttt{side: Vec}: Represents a sequence of steps for a side kick.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{get_steps}}: Retrieves the steps for a specific kick variant.
\item \textbf{\texttt{advance_to_next_step}}: Proceeds to the next step in a sequence.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn get_steps(&self, variant: KickVariant) -> &[KickStep];
\end{lstlisting}

\begin{lstlisting}
pub fn advance_to_next_step(self) -> Self;
\end{lstlisting}

\subsubsection*{Design Decisions:}
Organizing kick steps into separate sequences for each type of kick allows for flexibility in updating and customizing kick actions. This design supports easy expansions and modifications to support new kick types or step changes.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Usage by \texttt{Kick State}}: Provides the steps that the \texttt{Kick State} module executes to perform kicks smoothly.
\item \textbf{Coordination with \texttt{Motor Commands}}: Supplies precise joint movements to the motor command system for execution.
\end{itemize}

\textbf{Data Flow:}
Each kick sequence is processed to issue a series of motor commands that drive physical joint changes, ensuring the kick is performed accurately.

\subsection{Modes Module}
\textbf{File:} \texttt{mode.rs} and Submodules

\textbf{Purpose:} The Modes module manages the operational state of the walking engine, coordinating transitions between different behaviors such as standing, walking, and kicking. It serves as a central controller to ensure the system reacts appropriately to command inputs and sensory data.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Enum: \texttt{Mode}}
\begin{itemize}
\item \textbf{Variants:}
\begin{itemize}
\item \texttt{Standing(Standing)}: Represents the robot is in a standing position.
\item \texttt{Starting(Starting)}: Represents the initial step in the transition to walking.
\item \texttt{Walking(Walking)}: Represents ongoing walking activity.
\item \texttt{Kicking(Kicking)}: Represents the execution of a kicking action.
\item \texttt{Stopping(Stopping)}: Represents the cessation of movement.
\item \texttt{Catching(Catching)}: Represents handling and adjusting to sudden shifts, potentially from external interactions.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{stand}: Sets the engine mode to standing.
\begin{lstlisting}
fn stand(self, context: &Context) -> Mode;
\end{lstlisting}
\item \textbf{walk}: Initiates walking based on step commands.
\begin{lstlisting}
fn walk(self, context: &Context, step: Step) -> Mode;
\end{lstlisting}
\item \textbf{kick}: Initiates a kicking action within the current mode context.
\begin{lstlisting}
fn kick(self, context: &Context, variant: KickVariant, side: Side, strength: f32) -> Mode;
\end{lstlisting}
\end{itemize}
\end{itemize}
\end{itemize}

\subsubsection*{Design Decisions:}
Employing an enum for modes ensures that mode transitions are controlled and predictable. The explicit handling of different states allows for modular extensions, such as adding new modes or enhancing existing ones with minimal codebase disruption.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Interfaces with all major modules}: Modes dictate what actions are permissible and coordinate these actions with input from modules such as \texttt{Step Plan} and \texttt{Kick State}.
\item \textbf{Reactiveness to Contextual Changes}: Continuously evaluates external data inputs to determine state transitions. This setup allows, for example, automatic adjustment from walking to standing if an obstacle is detected.
\end{itemize}

\textbf{Data Flow:}
Input commands and sensor data are evaluated to determine the appropriate mode, steering further processing cycles based on this decision.

\subsection{Parameters Module}
\textbf{File:} \texttt{parameters.rs}

\textbf{Purpose:} The Parameters module contains configurable settings that influence the behavior and performance of the walking engine. By centralizing these settings, the system ensures consistent behavior adjustments across different modules based on external conditions or testing requirements.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{max_step_length: f32}: Determines the maximum allowable step length.
\item \texttt{stride_frequency: f32}: Defines the frequency of steps taken per second.
\item \texttt{arm_swing_amplitude: f32}: Controls the amplitude of arm swings to enhance balance.
\end{itemize}
\end{itemize}

\subsubsection*{Design Decisions:}
Centralizing parameter configurations allows for easy tuning and testing under various conditions without modifying the core logic of individual modules. This abstraction aids in swift adaptations to different robotic platforms or environment changes.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Influence on Step Plan}: Provides crucial values that determine step metrics and gait patterns.
\item \textbf{Integration with Gyro Balancing}: Adjusts balancing thresholds and responses to ensure stability across various speeds and terrains.
\end{itemize}

\subsection{Step Plan Module}
\textbf{File:} \texttt{step_plan.rs}

\textbf{Purpose:} The Step Plan module outlines the sequence and trajectory of steps for walking. It integrates environmental data and user parameters to produce a cohesive walking strategy that seeks to optimize speed, stability, and terrain handling.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{calculate_trajectory}}: Computes the path and timing of foot placements based on the current walking goal.
\item \textbf{\texttt{adjust_for_terrain}}: Modifies step parameters to accommodate changes in terrain slope or composition.
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn calculate_trajectory(&self, target_velocity: f32, environment_data: &Environment) -> StepPlan;
pub fn adjust_for_terrain(&mut self, terrain_type: TerrainType);
\end{lstlisting}

\subsubsection*{Design Decisions:}
By abstracting step planning into a dedicated module, the system can rapidly adapt the walking plan in response to sensory inputs or parameter changes, enhancing flexibility in dynamic scenarios.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Collaboration with Parameters}: Utilizes parameter values to define limits and preferences for each walking session.
\item \textbf{Integrated with Feet Module}: Provides requisite foot placement and trajectory data needed to execute the planned steps.
\end{itemize}

\textbf{Data Flow:}
Data from sensors and user inputs flow into this module, which processes and outputs structured step plans aligned with designated walking goals.

\subsection{Step State Module}
\textbf{File:} \texttt{step_state.rs}

\textbf{Purpose:} The Step State module maintains the state of each step being executed by the walking engine. It tracks the status and parameters of steps in real-time, ensuring coordinated movements and transitions.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{StepState}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{current_step: usize}: Index of the current step in the step sequence.
\item \texttt{duration: f32}: The time duration for the current step.
\item \texttt{is_completed: bool}: Flag indicating whether the current step is complete.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{update_state}}: Updates the step state based on elapsed time and current position.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn update_state(&mut self, elapsed_time: f32);
\end{lstlisting}

\subsubsection*{Design Decisions:}
Centralizing step state logic allows seamless tracking and coordination of movements, providing a clear mechanism for handling transitions and sequences within the walking engine.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Integration with Step Plan}: Collaborates with step planning logic to dynamically adjust timing and progression based on real-time conditions.
\item \textbf{Feedback to Modes}: Provides state feedback used by the \texttt{Modes} module to trigger transitions or status updates.
\end{itemize}

\subsection{Stiffness Module}
\textbf{File:} \texttt{stiffness.rs}

\textbf{Purpose:} The Stiffness module configures and applies stiffness settings to the robot's joints, optimizing flexibility, responsiveness, and support according to the current mode and operational needs.

\subsubsection*{Key Components:}
\begin{itemize}
\item \textbf{Struct: \texttt{StiffnessSettings}}
\begin{itemize}
\item \textbf{Attributes:}
\begin{itemize}
\item \texttt{leg_stiffness: f32}: Level of stiffness applied to the leg joints.
\item \texttt{arm_stiffness: f32}: Level of stiffness applied to the arm joints.
\end{itemize}
\item \textbf{Functions:}
\begin{itemize}
\item \textbf{\texttt{apply_stiffness}}: Applies the configured stiffness settings to the specified joints.
\end{itemize}
\end{itemize}
\end{itemize}

\begin{lstlisting}
pub fn apply_stiffness(&self, legs: f32, arms: f32) -> MotorCommands;
\end{lstlisting}

\subsubsection*{Design Decisions:}
By abstracting stiffness control, the module allows flexible configuration and dynamic adaptation to different tasks, environments, or robotic models, enhancing performance and safety.

\subsubsection*{Interactions:}
\begin{itemize}
\item \textbf{Collaboration with Modes}: Adjusts stiffness levels according to the current operational mode, ensuring optimal joint performance.
\item \textbf{Integration with Motor Commands}: Directly informs the \texttt{Motor Commands} to implement the desired stiffness during task execution.
\end{itemize}

\section{Flowchart}
The flowchart illustrates the structure and interactions between various modules involved in the system. Each module is responsible for specific functionalities related to robotic movement and control.
\subsection{Design Choices in the Flowchart}

\begin{itemize}
\item \textbf{Normal Arrows:}
Indicate standard data flow or direct invocation/interaction between modules, showcasing how main functionalities and processes are sequentially interconnected.

\item \textbf{Dashed Arrows:} 
Used for auxiliary or indirect interactions, where the relationship is not always active or represents peripheral influence, such as configuration settings or supporting processes.

\item \textbf{Color Usage:}
Modules depicted in different shades (blue for main modules and cyan for submodules) help distinguish between primary functional components and auxiliary submodules, emphasizing their hierarchical relationships.

\end{itemize}

\begin{center}
\begin{tikzpicture}[node distance=1.6cm and 3cm]
% Main Modules
\node (anatomic_constraints) [module] {anatomic_constraints.rs};
\node (arm) [module, below of=anatomic_constraints] {arm.rs};
\node (feet) [module, below of=arm] {feet.rs};
\node (foot_leveling) [module, below of=feet] {foot_leveling.rs};
\node (gyro_balancing) [module, below of=foot_leveling] {gyro_balancing.rs};
\node (kick_state) [module, below of=gyro_balancing] {kick_state.rs};

\node (kick_steps) [module, right of=anatomic_constraints, xshift=4cm] {kick\_steps.rs};
\node (lib) [module, below of=kick_steps] {lib.rs};
\node (parameters) [module, below of=lib] {parameters.rs};
\node (step_plan) [module, below of=parameters] {step\_plan.rs};
\node (step_state) [module, below of=step_plan] {step\_state.rs};
\node (stiffness) [module, below of=step_state] {stiffness.rs};

\node (mode) [module, below of=kick_state, xshift=4cm] {mode.rs};

% Mode Submodules arranged horizontally
\node (catching) [submodule, below of=mode, yshift=-2cm, xshift=-5cm] {catching.rs};
\node (kicking) [submodule, right of=catching, xshift=2.5cm] {kicking.rs};
\node (standing) [submodule, right of=kicking, xshift=2.5cm] {standing.rs};
\node (stopping) [submodule, right of=standing, xshift=2.5cm] {stopping.rs};
\node (starting) [submodule, below of=catching, yshift=-1.5cm] {starting.rs};
\node (walking) [submodule, right of=starting, xshift=7.5cm] {walking.rs};

% Arrows and Flow avoiding overlap
\draw [arrow] (anatomic_constraints) -- (arm);
\draw [arrow] (arm) -- (feet);
\draw [arrow] (feet) -- (foot_leveling);
\draw [arrow] (foot_leveling) -- (gyro_balancing);
\draw [arrow] (gyro_balancing) -- (kick_state);
\draw [arrow] (kick_state) -- (mode);

\draw [arrow] (kick_steps) -- (lib);
\draw [arrow] (lib) -- (parameters);
\draw [arrow] (parameters) -- (step_plan);
\draw [arrow] (step_plan) -- (step_state);
\draw [arrow] (step_state) -- (stiffness);

\draw [arrow] (mode) -- (catching);
\draw [arrow] (mode) -- (kicking);
\draw [arrow] (mode) -- (standing);
\draw [arrow] (mode) -- (stopping);

\draw [arrow] (catching) -- (starting);
\draw [arrow] (kicking) .. controls +(down:1cm) and +(up:1cm) .. (walking);
\draw [arrow] (standing) .. controls +(down:1cm) and +(up:1cm) .. (stopping);
\draw [arrow] (starting) -- (walking);
\draw [arrow] (stopping) .. controls +(down:1cm) and +(left:1cm) .. (walking);

% Additional arrows
\draw [dashedarrow] (kick_state) -- (stiffness);
\draw [dashedarrow] (stiffness) -- (gyro_balancing);

\end{tikzpicture}
\end{center}

\end{document}

Walking Engine Documentation and Flowchart.pdf