Merge pull request #16 from rdesarz/14-implement-foot-planner-to-generate-dynamically-the-trajectory-of-the-swinging-foot

- Add a new example to test dynamic walking controller instead of precomputed trajectories 
- Add a method to generate dynamically the swing foot trajectory 
- Split steps sequence computation and feet trajectories generation 
- Improve plot of contact forces 
- Add walking state machine to a new module 
- Update documentation

Author: rdesarz

biped_walking_controller/foot.py

@@ -9,15 +9,13 @@
 """
 
 import math
+
 import numpy as np
 import shapely
 from shapely import Polygon, Point, affinity, union
 from shapely.ops import nearest_points
 
-
-import math
-import numpy as np
-from typing import Tuple
+from biped_walking_controller.state_machine import WalkingStateMachineParams, Foot
 
 
 def bezier_quintic(P: np.ndarray, s: np.ndarray) -> np.ndarray:
@@ -185,15 +183,44 @@ def __call__(self, p_start: np.ndarray, p_end: np.ndarray, s: np.ndarray) -> np.
         return path
 
 
-def compute_feet_path_and_poses(
+def compute_steps_sequence(
+    rf_initial_pose: np.ndarray,
+    lf_initial_pose: np.ndarray,
+    n_steps: int,
+    l_stride: float,
+):
+    steps_pose = np.zeros((n_steps + 2, 3))
+    steps_pose[0] = rf_initial_pose
+    steps_ids = [Foot.RIGHT]
+    for i in range(1, n_steps + 1):
+        sign = -1.0 if i % 2 == 0 else 1.0
+        steps_ids.append(Foot.RIGHT if i % 2 == 0 else Foot.LEFT)
+        steps_pose[i] = np.array([i * l_stride, sign * math.fabs(lf_initial_pose[1]), 0.0])
+
+    # Add a last step to have both feet at the same level
+    steps_pose[-1] = steps_pose[-2]
+    steps_pose[-1][1] = steps_pose[-1][1] * -1.0
+
+    return steps_pose, steps_ids
+
+
+def compute_time_vector(t_ss, t_ds, t_init, t_final, n_steps, dt):
+    total_time = t_init + n_steps * (t_ss + t_ds) + (t_ss + t_final)
+    N = int(total_time / dt)
+    t = np.arange(N) * dt
+
+    return t
+
+
+def compute_feet_trajectories(
     rf_initial_pose,
     lf_initial_pose,
     n_steps,
+    steps_pose,
     t_ss,
     t_ds,
     t_init,
     t_final,
-    l_stride,
     dt,
     traj_generator=BezierCurveFootPathGenerator(foot_height=0.1),
 ):
@@ -221,7 +248,7 @@ def compute_feet_path_and_poses(
         Double-support duration between steps.
     t_init : float
         Initial DS duration before stepping.
-    t_final : float
+    t_end : float
         Final DS duration to center the CoM.
     l_stride : float
         Step length along +x for each new foothold.
@@ -258,30 +285,14 @@ def compute_feet_path_and_poses(
     # the same level and a double support step to  place the CoM in the
     # middle of the feet
 
-    total_time = t_init + n_steps * (t_ss + t_ds) + (t_ss + t_final)
-    N = int(total_time / dt)
-    t = np.arange(N) * dt
+    t = compute_time_vector(t_ss, t_ds, t_init, t_final, n_steps, dt)
+    N = len(t)
 
     # Initialize path
     rf_path = np.ones([N, 3]) * rf_initial_pose
     lf_path = np.ones([N, 3]) * lf_initial_pose
     phases = np.ones(N)
 
-    # Switch of the CoM to the right foot implies both feet stays at the same position
-    mask = t < t_init
-    rf_path[mask, :] = rf_initial_pose
-    lf_path[mask, :] = lf_initial_pose
-
-    steps_pose = np.zeros((n_steps + 2, 3))
-    steps_pose[0] = rf_initial_pose
-    for i in range(1, n_steps + 1):
-        sign = -1.0 if i % 2 == 0 else 1.0
-        steps_pose[i] = np.array([i * l_stride, sign * math.fabs(lf_initial_pose[1]), 0.0])
-
-    # Add a last step to have both feet at the same level
-    steps_pose[-1] = steps_pose[-2]
-    steps_pose[-1][1] = steps_pose[-1][1] * -1.0
-
     # Compute motion of left foot
     for k in range(0, n_steps, 2):
         t_begin = t_init + k * (t_ss + t_ds)
@@ -301,7 +312,7 @@ def compute_feet_path_and_poses(
 
         # # Add constant part till the next step
         t_begin = t_init + k * (t_ss + t_ds) + t_ss
-        t_end = total_time
+        t_end = t[-1]
         mask = (t >= t_begin) & (t < t_end)
         lf_path[mask, 0] = steps_pose[k + 1][0]
 
@@ -324,11 +335,11 @@ def compute_feet_path_and_poses(
 
         # # Add constant part till the next step
         t_begin = t_init + k * (t_ss + t_ds) + t_ss
-        t_end = total_time
+        t_end = t[-1]
         mask = (t >= t_begin) & (t < t_end)
         rf_path[mask, 0] = steps_pose[k + 1][0]
 
-    return t, lf_path, rf_path, steps_pose, phases
+    return t, lf_path, rf_path, phases
 
 
 def clamp_to_polygon(pnt: np.ndarray, poly: Polygon):
@@ -442,3 +453,37 @@ def get_active_polygon(t: float, steps_pose, t_ss: float, t_ds: float, foot_shap
         return compute_single_support_polygon(steps_pose[-1], foot_shape)
     else:
         return compute_double_support_polygon(steps_pose[i], steps_pose[i + 1], foot_shape)
+
+
+def compute_swing_foot_pose(
+    t_state: float,
+    params: WalkingStateMachineParams,
+    step_start: np.ndarray,
+    step_target: np.ndarray,
+    touchdown_extension_vel: float,
+    path_generator,
+) -> np.ndarray:
+    """
+    Compute swing foot pose for current state time with late touchdown extension.
+
+    - For t_state in [0, t_ss]: nominal min-jerk swing between step_start and step_target.
+    - For t_state > t_ss and no contact: keep moving the *commanded* foot down
+      below the nominal ground height at constant velocity.
+      The physics/contact solver will clamp penetration.
+    - As soon as contact_force > force_threshold: freeze at step_target.
+    """
+    t_ss = params.t_ss
+
+    if t_state <= t_ss:
+        # Normal swing phase
+        s = np.clip(t_state / t_ss, 0.0, 1.0)
+
+        return path_generator(step_start, step_target, s)
+
+    # Late-touchdown extension phase
+    # We assume step_target[2] is the nominal ground height.
+    # After nominal end of swing, keep commanding the foot below that plane.
+    dt = t_state - t_ss
+    pos = step_target.copy()
+    pos[2] = step_target[2] - touchdown_extension_vel * dt
+    return pos

biped_walking_controller/plot.py

@@ -2,8 +2,7 @@
     compute_double_support_polygon,
     compute_single_support_polygon,
 )
-
-from biped_walking_controller.preview_control import WalkingState
+from biped_walking_controller.state_machine import WalkingState
 
 
 def plot_steps(axes, steps_pose, step_shape):
@@ -188,7 +187,7 @@ def traj2d(arr):  # drop last 2 samples as in your code
     ax2.set_title(f"{title_prefix} — plan view (x–y)")
 
 
-def plot_contact_forces(
+def plot_contact_forces_and_state(
     t: float,
     force_rf: float,
     force_lf: float,
@@ -200,18 +199,24 @@ def plot_contact_forces(
     force_lf = np.asarray(force_lf).ravel()
     assert t.size == force_rf.size == force_lf.size
 
-    fig, ax = plt.subplots(2, figsize=(12, 8), layout="constrained")
-    ax[0].plot(t, force_rf, label="right foot")
-    ax[0].plot(t, force_lf, label="left foot")
-    ax[0].set_xlabel("t [s]")
+    fig, ax = plt.subplots(2, figsize=(12, 8), layout="constrained", sharex=True)
+    ax[0].plot(t, force_rf, label="right foot", marker="*")
+    ax[0].plot(t, force_lf, label="left foot", marker="*")
     ax[0].set_ylabel("Normal force [N]")
-    ax[0].set_title(title)
+    ax[0].set_title("Contact Force Fx")
     ax[0].grid(True)
     ax[0].legend()
 
-    ax[1].plot(t, states, label="state")
+    ticks = []
+    labels = []
+    for data in WalkingState:
+        ticks.append(data.value)
+        labels.append(data.name)
+
+    ax[1].plot(t, states, label="state", marker="*")
     ax[1].set_xlabel("t [s]")
     ax[1].set_ylabel("State [-]")
-    ax[1].set_title(title)
+    ax[1].set_title("State")
+    ax[1].set_yticks(ticks, labels)
     ax[1].grid(True)
     ax[1].legend()