Small fix

Author: rdesarz

biped_walking_controller/foot.py

@@ -175,11 +175,11 @@ def __call__(self, p_start: np.ndarray, p_end: np.ndarray, s: np.ndarray) -> np.
         np.ndarray
             Sampled path of shape ``(N, 3)``.
         """
-        path = np.zeros(3)
+        path = np.zeros((len(s), 3))
         theta = s * math.pi
-        path[2] = np.sin(theta) * self.foot_height
-        path[1] = p_start[1]
-        path[0] = (1.0 - s) * p_start[0] + s * p_end[0]
+        path[:, 2] = np.sin(theta) * self.foot_height
+        path[:, 1] = p_start[1]
+        path[:, 0] = (1.0 - s) * p_start[0] + s * p_end[0]
         return path
 
 
@@ -455,48 +455,11 @@ def get_active_polygon(t: float, steps_pose, t_ss: float, t_ds: float, foot_shap
         return compute_double_support_polygon(steps_pose[i], steps_pose[i + 1], foot_shape)
 
 
-def min_jerk_scalar(s: float) -> float:
-    """
-    Minimum-jerk scalar polynomial on [0, 1].
-    """
-    s2 = s * s
-    s3 = s2 * s
-    return 10.0 * s3 * s2 - 15.0 * s2 * s2 + 6.0 * s3
-
-
-def swing_foot_min_jerk(
-    s: float,
-    p0: np.ndarray,
-    p1: np.ndarray,
-    swing_height: float,
-) -> np.ndarray:
-    """
-    Minimum-jerk 3D swing trajectory between p0 and p1 with vertical lift.
-
-    s in [0, 1]: normalized phase of the swing.
-    """
-    s = float(np.clip(s, 0.0, 1.0))
-    # Horizontal: simple min-jerk blend
-    alpha = min_jerk_scalar(s)
-    pos_xy = (1.0 - alpha) * p0[:2] + alpha * p1[:2]
-
-    # Vertical: lift up then back down via a simple "bump"
-    # z(s) = (1 - alpha)*z0 + alpha*z1 + h * 4*s*(1-s)
-    bump = 4.0 * s * (1.0 - s)
-    z = (1.0 - alpha) * p0[2] + alpha * p1[2] + swing_height * bump
-
-    out = np.empty(3)
-    out[0:2] = pos_xy
-    out[2] = z
-    return out
-
-
 def compute_swing_foot_pose(
     t_state: float,
     params: WalkingStateMachineParams,
     step_start: np.ndarray,
     step_target: np.ndarray,
-    swing_height: float,
     touchdown_extension_vel: float,
     path_generator,
 ) -> np.ndarray:

examples/example_5_walking_controller.py

@@ -273,7 +273,6 @@ def main():
                 params=params,
                 step_start=lf_start,
                 step_target=lf_target,
-                swing_height=max_height_foot,
                 touchdown_extension_vel=touchdown_vel,
                 path_generator=foot_path_generator,
             )
@@ -304,7 +303,6 @@ def main():
                 params=params,
                 step_start=rf_start,
                 step_target=rf_target,
-                swing_height=max_height_foot,
                 touchdown_extension_vel=touchdown_vel,
                 path_generator=foot_path_generator,
             )