Add ZMP reference generation

Author: rdesarz

biped_walking_controller/preview_control.py

@@ -88,6 +88,41 @@ def compute_zmp_ref(
     return zmp_ref
 
 
+def build_zmp_horizon(
+    t, com_initial_pose, steps, ss_t, ds_t, t_init, t_final, interp_fn=cubic_spline_interpolation
+):
+    T = len(t)
+    zmp_ref = np.zeros([T, 2])
+
+    # Step on the first foot
+    mask = t < t_init
+    alpha = t[mask] / t_init
+    zmp_ref[mask, :] = interp_fn(alpha, com_initial_pose, steps[0])
+
+    # Alternate between foot
+    for idx, (current_step, next_step) in enumerate(zip(steps[:-2], steps[1:-1])):
+        # Compute current time range
+        t_start = t_init + idx * (ss_t + ds_t)
+
+        # Add single support phase
+        zmp_ref[(t >= t_start) & (t < t_start + ss_t)] = current_step
+
+        # Add double support phase
+        mask = (t >= t_start + ss_t) & (t < t_start + ss_t + ds_t + t[1])
+        alpha = (t[mask] - (t_start + ss_t)) / ds_t
+        zmp_ref[mask, :] = interp_fn(alpha, current_step, next_step)
+
+    # Last phase: SS on last-but-one, then blend to midpoint of last two
+    t_start = t_init + (len(steps) - 2) * (ss_t + ds_t)
+    zmp_ref[(t >= t_start) & (t < t_start + ss_t)] = steps[-2]
+
+    mask = (t >= t_start + ss_t) & (t < t_start + ss_t + t_final)
+    alpha = (t[mask] - (t_start + ss_t)) / t_final
+    zmp_ref[mask, :] = interp_fn(alpha, steps[-2], (steps[-1] + steps[-2]) / 2.0)
+
+    return zmp_ref
+
+
 @dataclass
 class PreviewControllerParams:
     """