Foot trajectory generation

Swing foot trajectory generation

Walking is organized into fixed-duration phases: Single Support (SS) and Double Support (DS). In Single Support, one foot is the swing foot and the other is the stance foot. In Double Support both feet are in contact and no swing foot exists. The timing of each phase is related to the trajectory of the ZMP. During Single Support phase, the ZMP position is maintained over the stance foot. During Double Support phase, the ZMP position moves from a foot to another. Currently, there are two approaches can be used to generate the swing foot trajectory: * The first one is to follow a sinusoidal time law along the world x-axis from \(x_0\) to \(x_1\) over duration \(T_{SS}\). * The second, default option uses a minimum-jerk Bézier curve whose velocity and acceleration are aligned with the ground normal. This ensures smooth foot motion. The curve parameters are set so that its peak reaches the desired maximum height.

The foot orientation is kept constant with yaw=0; the sole remains parallel to the floor. All phase durations are configurable.

The figure below shows the generated trajectory for both foot with a height of 20cm:

desc

Example

You can reproduce the example displayed on the figure by launching the script example_2_feet_motion.py. We recommend you to use Docker as explained in the installation part:

xhost +local:root
docker run --rm -it \
  --env DISPLAY \
  -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
  --device /dev/dri:/dev/dri \
  ghcr.io/rdesarz/biped-walking-controller \
  python examples/example_2_feet_motion.py

The parameters used in this script are the following:

dt = 0.005  # Delta of time of the model simulation
t_ss = 0.7  # Single support phase time window
t_ds = 0.8  # Double support phase time window
t_init = 2.0  # Initialization phase (transition from still position to first step)
t_end = 2.0 # Final phase (transition from walking to standstill position)
lf_initial_pose = np.array([0.0, 0.1, 0.0]) # Initial position of the left foot
rf_initial_pose = np.array([0.0, -0.1, 0.0]) # Initial position of the right foot
n_steps = 5 # Number of steps
l_stride = 0.3 # Length of the stride
max_height_foot = 0.2 # Maximal height of the foot during motion of the swing foot

Code API

Footstep geometry and trajectories utilities.

Provides:

Polygon operations to compute single/double-support regions.
ZMP clamp to the current support polygon.
Deterministic swing-foot trajectories and step poses.

BezierCurveFootPathGenerator

BezierCurveFootPathGenerator(foot_height)

Swing-foot path generator using a quintic Bézier with zero vel/acc at endpoints.

This generator fixes the first three control points at the start pose and the last three at the end pose, then sets the vertical components of the interior points to reach a prescribed apex height. The vertical “shape” parameter alpha is searched once at construction, then reused per call.

Parameters:

Name	Type	Description	Default
`foot_height`	`float`	Desired apex height of the swing foot above the line segment connecting start and end poses.	required

Attributes:

Name	Type	Description
`alpha`	`float`	Internal vertical shaping parameter calibrated so that the curve height at `s=0.5` equals `foot_height`.

Notes

The constructor performs a simple grid search over alpha in [0, 2*foot_height] to match the apex within 1e-3.
Endpoints have zero velocity and acceleration due to repeated control points: P0=P1=P2 and P3=P4=P5.

SinusoidFootPathGenerator

SinusoidFootPathGenerator(foot_height)

Swing-foot path generator with sinusoidal vertical profile.

The horizontal motion is linear from start to end along x, with y held constant at the start value. The vertical component follows: z(s) = foot_height * sin(pi * s)

Parameters:

Name	Type	Description	Default
`foot_height`	`float`	Maximum height at mid-swing.	required

Notes

This profile is C1 at the endpoints (zero velocity) but not C2 like the Bézier construction. Use the Bézier for smoother contact transitions.

bezier_quintic

bezier_quintic(P, s)

Evaluate a quintic (degree-5) Bézier curve at parameter values s.

A quintic Bézier curve is: B(s) = Σ_{i=0..5} C(5,i) (1-s)^{5-i} s^i P_i

Parameters:

Name	Type	Description	Default
`P`	`ndarray`	Control points of shape `(6, 3)`. Rows are `P0..P5`. Columns are Cartesian coordinates `[x, y, z]`.	required
`s`	`ndarray`	1D array of parameter values in `[0, 1]` of shape `(N,)`.	required

Returns:

Type	Description
`ndarray`	Curve samples of shape `(N, 3)`.

Notes

With P0=P1=P2 and P3=P4=P5, the curve has zero velocity and acceleration at both ends, which is the typical “minimum-jerk” boundary condition used for swing-foot profiles.

compute_feet_trajectories

compute_feet_trajectories(rf_initial_pose, lf_initial_pose, n_steps, steps_pose, t_ss, t_ds, t_init, t_final, dt, traj_generator=BezierCurveFootPathGenerator(foot_height=0.1))

Generate swing trajectories and step poses for alternating feet.

Scenario

Start with a DS phase of duration t_init to shift CoM. Then for n_steps, alternate SS (duration t_ss) and DS (duration t_ds) with forward stride l_stride. Finish with one SS to align both feet, then a final DS of duration t_final to center CoM.

Parameters:

Name	Type	Description	Default
`rf_initial_pose`	`(array - like, shape(3))`	Initial right foot pose proxy [x, y, z].	required
`lf_initial_pose`	`(array - like, shape(3))`	Initial left foot pose proxy [x, y, z].	required
`n_steps`	`int`	Number of forward steps.	required
`t_ss`	`float`	Single-support duration per step.	required
`t_ds`	`float`	Double-support duration between steps.	required
`t_init`	`float`	Initial DS duration before stepping.	required
`t_end`	`float`	Final DS duration to center the CoM.	required
`l_stride`	`float`	Step length along +x for each new foothold.	required
`dt`	`float`	Time discretization step.	required
`max_height_foot`	`float`	Peak swing height for the swinging foot.	required

Returns:

Name	Type	Description
`t`	`(ndarray, shape(N))`	Time samples.
`lf_path`	`(ndarray, shape(N, 3))`	Left foot trajectory [x, y, z].
`rf_path`	`(ndarray, shape(N, 3))`	Right foot trajectory [x, y, z].
`steps_pose`	`(ndarray, shape(n_steps + 2, 2))`	Planned foothold XY positions. Last row mirrors penultimate y to align feet.
`phases`	`(ndarray, shape(N))`	Phase flag over time: -1 for left swing, +1 for right swing, 1 during right-stance, -1 during left-stance, unchanged during DS.

Notes

Swing z uses a half-sine profile with peak max_height_foot.
X is linearly interpolated during SS, then held constant until the next step.
Y remains fixed to the initial lateral offsets.

clamp_to_polygon

clamp_to_polygon(pnt, poly)

Project a 2D point to the closest location inside a polygon.

Parameters:

Name	Type	Description	Default
`pnt`	`(ndarray, shape(2))`	Input point [x, y].	required
`poly`	`Polygon`	Support polygon in world coordinates.	required

Returns:

Type	Description
`(ndarray, shape(2))`	Point inside `poly`. Equals `pnt` if already inside.

Notes

Uses the nearest point on the polygon exterior if outside.

compute_double_support_polygon

compute_double_support_polygon(foot_pose_a, foot_pose_b, foot_shape)

Compute the convex support region for double support.

Parameters:

Name	Type	Description	Default
`foot_pose_a`	`(array - like, shape(2) or (3,))`	World pose proxy for foot A: uses [x, y].	required
`foot_pose_b`	`(array - like, shape(2) or (3,))`	World pose proxy for foot B: uses [x, y].	required
`foot_shape`	`Polygon`	Foot contact shape in the foot frame (centered).	required

Returns:

Type	Description
`Polygon`	Convex hull of the union of both translated foot polygons.

compute_single_support_polygon

compute_single_support_polygon(foot_pose, foot_shape)

Compute the support region for single support.

Parameters:

Name	Type	Description	Default
`foot_pose`	`(array - like, shape(2) or (3,))`	World pose proxy for the stance foot: uses [x, y].	required
`foot_shape`	`Polygon`	Foot contact shape in the foot frame (centered).	required

Returns:

Type	Description
`Polygon`	Translated foot polygon in world coordinates.

get_active_polygon

get_active_polygon(t, steps_pose, t_ss, t_ds, foot_shape)

Select the active support polygon at time t.

Parameters:

Name	Type	Description	Default
`t`	`float`	Elapsed time since start.	required
`steps_pose`	`(array - like, shape(K, 2))`	Footstep sequence in world XY. Index 0 is the first stance (right or left).	required
`t_ss`	`float`	Single-support duration per step.	required
`t_ds`	`float`	Double-support duration between steps.	required
`foot_shape`	`Polygon`	Foot contact shape in the foot frame (centered).	required

Returns:

Type	Description
`Polygon`	Single- or double-support polygon active at time `t`.

Notes

Step period t_step = t_ss + t_ds.
For the last period, returns single support on the last foot.

compute_swing_foot_pose

compute_swing_foot_pose(t_state, params, step_start, step_target, touchdown_extension_vel, path_generator)

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.