Go2 Jump¶
examples/trajgen/python/go2/go2_jump.py
generates a jumping trajectory for the Go2 quadruped: a stance phase, a flight phase with all four feet off the
ground, and a landing back into a specified footprint. Where the previous examples used a single, unchanging set of
costs and constraints across the whole horizon, a jump needs the problem itself to change over time: which
constraints even apply (a foot mid-air obviously can't be held in a friction cone) depends on which stage you're
looking at. This is the first example built around contact scheduling, per-stage enabling and disabling of
contact constraints, and it also demonstrates the object-based (as opposed to keyword-argument) style of
configuring solver parameters. It builds directly on Panda Circle.
Setup¶
import ssqpy
import ssqpy.model.costs as costs
import ssqpy.model.constraints as constraints
import numpy as np
import argparse
from math import pi
"""
Go2 Jump example
Performs a simple jump with the Go2 robot
"""
# Model Parameters
HORIZON = 150
dt = 0.01
def parse_args():
parser = argparse.ArgumentParser(
description="Go2 jump trajectory generation"
)
parser.add_argument(
"--start-step",
type=int,
default=60,
help="Step at which the robot leaves the ground",
)
parser.add_argument(
"--end-step",
type=int,
default=90,
help="Step at which the robot lands",
)
parser.add_argument(
"-o",
"--output",
default="../../data/go2_jump.json",
help="Path of the output JSON file",
)
return parser.parse_args()
args = parse_args()
START_STEP = args.start_step
END_STEP = args.end_step
# Weights
A_WGT = 1e-3
V_WGT = 1e-2
U_WGT = 1e-4
F_WGT = 1e-3
C_WGT = 1e-3
MAX_NORMAL_FORCE = 100
# Create an SSQP model for the Go2
model = ssqpy.model.Model(
HORIZON,
dt,
from_preset=ssqpy.model.Preset.Go2,
solver_mode=ssqpy.model.SolverMode.ReducedInverseDynamics,
)
nq = model.getnq()
nv = model.getnv()
nu = model.getnu()
ReducedInverseDynamics solver mode is used, with the Go2
preset (which fixes free_flyer=True for you) since the
Go2's base isn't bolted to anything: it's a free-floating rigid body (position + orientation on \(SE(3)\), another
instance of the manifold-valued states SSQP
handles) whose motion is determined entirely by the forces the legs exert on the ground, not by any actuator
directly driving the base. ReducedInverseDynamics (rather than the plain InverseDynamics used for the fully
actuated Panda arm) accounts for exactly this: the base's 6 degrees of freedom are unactuated, so they're enforced
through the dynamics rather than treated as free control inputs.
START_STEP/END_STEP mark the flight window (stages 60–89 of a 150-stage horizon) during which no foot is in
contact with the ground. MAX_NORMAL_FORCE caps how hard each foot is allowed to push, which matters once contact
forces become part of the optimization below.
Initial state and a static-equilibrium initial guess¶
# Initial and final positions
base_pos = np.array([0.0, 0.0, 0.3879793820122])
base_config = np.array([0.0, pi / 6, -pi / 3] * 4)
# Initial configuration
init = np.zeros(nq + nv)
init[:3] = base_pos
init[6] = 1.0
init[7:19] = base_config
init_control = np.zeros(nu + 12)
init_control[-10] = 3.75 * 9.81
init_control[-7] = 3.75 * 9.81
init_control[-4] = 3.75 * 9.81
init_control[-1] = 3.75 * 9.81
base_config is a standing crouch pose, repeated for all four legs (each leg's three joints get the same
hip/thigh/calf angles, since the robot starts symmetric). init_control is not zero: each foot's vertical contact
force is initialized to roughly a quarter of the robot's weight (\(3.75 \times 9.81\ \text{N}\)), i.e. a static
standing equilibrium, a much better initial guess for the stance stages than zero. This matters more here than in
the earlier examples: a jump is a highly dynamic, contact-rich motion, and starting the solver's guess for the
forces at "the robot is already balanced on all four feet" rather than "no forces at all" avoids wasting early
iterations just discovering that a standing robot needs to push against the ground to not fall over.
Costs and reusable constraint objects¶
joint_angle_cost = costs.SquaredJointAngleCost(model, A_WGT)
joint_vel_cost = costs.SquaredJointVelocityCost(model, V_WGT)
control_cost = costs.SquaredControlCost(model, U_WGT)
force_cost = costs.SquaredForceCost(model, F_WGT)
config_cost = costs.SquaredConfigurationErrorCost(model, base_config, C_WGT)
fl_foot_pos_bound = constraints.FrameTranslationBound(
model,
"FL_foot",
np.array((-np.inf, 0.1, -0.01)),
np.array((np.inf, np.inf, np.inf)),
)
fr_foot_pos_bound = constraints.FrameTranslationBound(
model,
"FR_foot",
np.array((-np.inf, -np.inf, -0.01)),
np.array((np.inf, -0.1, np.inf)),
)
rl_foot_pos_bound = constraints.FrameTranslationBound(
model,
"RL_foot",
np.array((-np.inf, 0.1, -0.01)),
np.array((np.inf, np.inf, np.inf)),
)
rr_foot_pos_bound = constraints.FrameTranslationBound(
model,
"RR_foot",
np.array((-np.inf, -np.inf, -0.01)),
np.array((np.inf, -0.1, np.inf)),
)
head_constraint = constraints.FrameTranslationBound(
model,
"Head_lower",
np.array((-np.inf, -np.inf, 0.1)),
np.array((np.inf, np.inf, np.inf)),
)
init_controls = []
Regularization costs (joint angle, velocity, control, force, and a configuration-tracking cost pulling toward the crouch pose) are created once, to be reused across stages, exactly as in the earlier examples. What's new here is that per-foot position bounds are also created upfront, before the per-stage loop, because the same constraint objects will be attached to every stage:
-
The position bounds keep each foot within its own quadrant of the body frame (preventing legs from crossing, e.g.
FL_foot's y-coordinate is bounded to \(y \geq 0.1\), keeping it on the robot's left side throughout). -
head_constraintkeeps the head above a minimum ground clearance throughout, stance and flight alike, so the jump doesn't solve itself by having the robot faceplant.
The per-stage loop: stance, flight, stance¶
for k in range(HORIZON):
# Add regularization costs
model.addCost(k, joint_angle_cost)
model.addCost(k, joint_vel_cost)
model.addCost(k, control_cost)
model.addCost(k, force_cost)
model.addCost(k, config_cost)
model.addConstraints(k, fl_foot_pos_bound)
model.addConstraints(k, fr_foot_pos_bound)
model.addConstraints(k, rl_foot_pos_bound)
model.addConstraints(k, rr_foot_pos_bound)
model.addConstraints(k, head_constraint)
if k < START_STEP or k >= END_STEP:
t1 = model.addContactConstraint(
k, "FL_foot", max_normal_force=MAX_NORMAL_FORCE, eq_tol=0.01
)
t1.set_enabled(False)
t2 = model.addContactConstraint(
k, "FR_foot", max_normal_force=MAX_NORMAL_FORCE, eq_tol=0.01
)
# t2.set_enabled(False)
t3 = model.addContactConstraint(
k, "RL_foot", max_normal_force=MAX_NORMAL_FORCE, eq_tol=0.01
)
# t3.set_enabled(False)
t4 = model.addContactConstraint(
k, "RR_foot", max_normal_force=MAX_NORMAL_FORCE, eq_tol=0.01
)
# t4.set_enabled(False)
init_controls.append(init_control)
continue
init_controls.append(np.zeros(nu))
Every stage gets the regularization costs and the position/head bounds unconditionally; those don't depend on
whether the robot is airborne. The contact scheduling itself happens in the if k < START_STEP or k >= END_STEP
branch, i.e. outside the flight window: for each foot, Model.addContactConstraint adds both a
ContactConstraint (friction cone, so the foot can only push, not pull, and only within a friction-limited cone of
directions, plus non-penetration, so the foot can't go through the ground, plus a no-slip zero-velocity bound, so
a planted foot can't slide along it) and the matching ground-reaction force in one call. These are exactly the
constraints that make "standing" or "walking" possible: without a friction cone, a solved trajectory could have
feet pushing sideways with unlimited force; without non-penetration, a foot could dip below the terrain to reduce
cost elsewhere; without the no-slip bound, a stance foot could skate across the ground.
Inside the flight window (the branch that continues past this block), no contact constraints are added at
all: no friction cone, no non-penetration, no zero-velocity bound, for any foot. With nothing tying the feet to
the ground, the dynamics reduce to free-falling under gravity for those stages, exactly as a jump should be; the
solver isn't told "be airborne", it simply has no way to keep the feet in contact during that window, so the
physics does the rest.
Note that this particular script explicitly disables FL's contact constraint (including its no-slip bound) even
during the "stance" branch (t1.set_enabled(False)) while leaving FR/RL/RR enabled, a reminder that contacts are toggled
independently per foot per stage, not just per phase: nothing in the API requires all four feet to share the same
schedule, which is what makes asymmetric gaits (a limp, a three-legged stance, a diagonal trot) representable with
the same building blocks. set_enabled(False) also demonstrates the other way to keep a constraint out of the
problem for a given stage: instead of only calling addConstraints when a constraint should apply (as done for
the other three feet, conditionally, by virtue of being inside the if branch at all), a constraint can be added
unconditionally and toggled on/off afterward, which is convenient when a constraint's applicability depends on
something more complex than a single stage-index check.
Terminal stage: settle into a landing footprint¶
model.addCost(HORIZON, joint_vel_cost)
model.addConstraints(
HORIZON,
constraints.FrameLinearVelocityBound(
model, "base", np.zeros(3), np.zeros(3)
),
)
model.addConstraints(HORIZON, head_constraint)
model.addConstraints(
HORIZON,
constraints.FrameTranslationBound(
model,
"FL_foot",
np.array((0.1934, 0.142, -0.00002682)),
np.array((0.1934, 0.142, -0.00002682)),
),
)
model.addConstraints(
HORIZON,
constraints.FrameTranslationBound(
model,
"FR_foot",
np.array((0.1934, -0.142, -0.00002682)),
np.array((0.1934, -0.142, -0.00002682)),
),
)
model.addConstraints(
HORIZON,
constraints.FrameTranslationBound(
model,
"RL_foot",
np.array((-0.1934, 0.142, -0.00002682)),
np.array((-0.1934, 0.142, -0.00002682)),
),
)
model.addConstraints(
HORIZON,
constraints.FrameTranslationBound(
model,
"RR_foot",
np.array((-0.1934, -0.142, -0.00002682)),
np.array((-0.1934, -0.142, -0.00002682)),
),
)
# Finalize the model with our state and controls
model.finalize(init_states=init)
At the terminal stage, a zero base-velocity constraint asks the robot to be at rest, and each foot gets a
FrameTranslationBound with equal lower and upper bounds, an equality constraint (see
Constraints in Costs, Constraints & Dynamics) pinning each
foot to an exact landing position, symmetric front-to-back and left-to-right around the base. Without this, the
solver has no reason to prefer landing back in a stable, symmetric stance over any other configuration that happens
to be feasible at stage H; pinning the feet gives the last portion of the trajectory (after END_STEP, back
inside the stance branch) something concrete to converge back onto.
Solving, with object-based parameter configuration¶
# SSQP model parameters
ssqp_params = ssqpy.solvers.ssqpParams()
ssqp_params.loose_success = True
ssqp_params.tolerance = 1e-2
ssqp_params.qp_solver_type = ssqpy.solvers.QPSolverType.HPIPM
admm_params = ssqpy.solvers.admmParams()
admm_params.abs_tolerance = 1e-3
admm_params.rel_tolerance = 1e-3
admm_params.max_iter = 25
admm_params.reset_rho = False
admm_params.reset_y = False
admm_params.warm_start = False
ssqp_params.admmParams = admm_params
hpipm_params = ssqpy.solvers.hpipmParams()
hpipm_params.max_iter = 25
hpipm_params.split_step = True
hpipm_params.prediction_correction = True
hpipm_params.tol_stat = 1e-3
hpipm_params.tol_comp = 1e-3
hpipm_params.tol_eq = 1e-3
hpipm_params.tol_ineq = 1e-3
ssqp_params.hpipmParams = hpipm_params
# Create a solver with the model
solver = ssqpy.solvers.ssqp(model, ssqp_params)
state, x, u = solver.solve()
print("Solver state:", state)
solver.write(args.output)
Rather than passing admm_/hpipm_-prefixed keyword arguments (as in Pendubot), this example
constructs ssqpParams, admmParams and hpipmParams objects directly and assigns fields on them before passing
the top-level ssqp_params to the solver; this is equivalent, just a different style, useful when parameters are
computed or reused across multiple solver instances. ssqp_params.loose_success = True is worth calling out
specifically: for a problem this constrained and dynamic, waiting for the strict Lagrangian-norm tolerance to be
met on every single term can take far longer than reaching a solution that's already good enough for the jump to
look and behave correctly; loose success lets SSQP stop once progress has stalled rather than keep grinding on
diminishing returns.
Run it (from examples/trajgen/python/go2/, with the environment set up per Installation):
The script prints the final solver state and writes ../../data/go2_jump.json. This is the example most worth visualizing to
actually understand: load the result with the visualizer example and watch the base crouch,
launch, tuck through flight, and land, with FL's foot behaving differently from the other three throughout stance
if you left the set_enabled(False) call in place.
Things to try¶
-
Comment out
t1.set_enabled(False)so all four feet share the same stance/flight schedule, and compare the landing symmetry against the default (asymmetric FL) version. -
Shrink the flight window (bring
--start-stepand--end-stepcloser together) for a shorter hop, or widen it for a higher one, and see how farMAX_NORMAL_FORCEand the joint/velocity regularization weights can be pushed before the solver stops converging. -
Change the terminal foot-position targets to land in a different footprint (e.g. narrower or wider stance) than the one the robot started in.
-
Try setting
admm_params.reset_rho/admm_params.reset_yback to their defaults (True) instead of theFalseused here, and compare convergence; these control whether ADMM's internal state carries over between the different QP formulations solved across SSQP iterations.