Quickstart

This walks through the smallest complete SSQP problem: driving a 1D double integrator to a target position, with custom (non-robot) dynamics and a custom cost. It corresponds to examples/trajgen/python/di.py; the code blocks below are pulled live from that file at doc build time.

Make sure ssqpy is importable first; see Installation.

1. Define the dynamics

Custom dynamics subclass ssqpy.model.dynamics.StageDynamics and must provide, at minimum, output_size, state_size, control_size, evaluate and gradient (Hessian-vector products are optional, defaulting to zero, and only needed if you want exact second-order information):

examples/trajgen/python/di.py

# Horizon and time step
H, dt = 10, 0.1


# Discrete double integrator, with the acceleration as the control
class DoubleIntegrator(ssqpy.model.dynamics.StageDynamics):
    def output_size(self):
        return 2

    def state_size(self):
        return 2

    def control_size(self):
        return 1

    def evaluate(self, x, u):
        p, v = x[:]
        vn = v + u[0] * dt
        pn = p + vn * dt

        return np.array([pn, vn])

    def gradient(self, x, u):
        dX = np.zeros((2, 2))
        dX[0, 0] = 1.0
        dX[0, 1] = dt
        dX[1, 1] = 1.0

        dU = np.zeros((2, 1))
        dU[0, 0] = dt**2
        dU[1, 0] = dt

        return dX, dU

    def hessprod_xx(self, x, u, lambda_):
        return np.zeros((2, 2))

    def hessprod_xu(self, x, u, lambda_):
        return np.zeros((2, 1))

    def hessprod_uu(self, x, u, lambda_):
        return np.zeros((1, 1))

The state is \([p, v]\) (position, velocity), the control is a scalar acceleration \(u\), and evaluate implements semi-implicit Euler integration with \(dt = 0.1\).

2. Define the cost

Custom costs subclass ssqpy.model.costs.StageCost and must provide evaluate, gradient, hessian_xx, hessian_xu and hessian_uu:

examples/trajgen/python/di.py

# Drives the position towards 1
class Cost(costs.StageCost):
    def evaluate(self, x, u):
        return 0.5 * (x[0] - 1.0) ** 2

    def gradient(self, x, u):
        dx = np.zeros(2)
        dx[0] = x[0] - 1.0

        du = np.zeros(1)

        return dx, du

    def hessian_xx(self, x, u):
        Hxx = np.zeros((2, 2))
        Hxx[0, 0] = 1.0

        return Hxx

    def hessian_xu(self, x, u):
        return np.zeros((2, 1))

    def hessian_uu(self, x, u):
        return np.zeros((1, 1))

This penalizes deviation of the position \(p\) from a target of \(1.0\), applied both at every running stage and at the terminal stage.

3. Build the model and solve

examples/trajgen/python/di.py

stage_cost = Cost()

model = ssqpy.model.Model(H, dt)

model.finalize(
    dynamics=[DoubleIntegrator()] * (H + 1),
    stage_cost=stage_cost,
    terminal_cost=stage_cost,
)

# Create an SSQP solver for the model
solver = ssqpy.solvers.ssqp(model)

state, x, u = solver.solve()

print("Solver state:", state)

print("x:")
x.print()
print("u:")
u.print()

model.finalize takes one dynamics object per stage, so H + 1 entries; since these dynamics are the same at every stage, the same object is repeated (as above). See Model for the full set of finalize arguments (XML loading, joint limits, initial guesses, etc.).

solver.solve() returns the solver's SolverState, and the resulting state and control trajectories as FlatVec objects, flattened, stagewise vectors that can be indexed with .stage(k) or converted with .toarray().

Next steps

• Model: horizons, XML-based robot models, solver modes and integration schemes

• Costs, Constraints & Dynamics: the stagewise function interface, and predefined costs/constraints that ship with SSQP

• The SSQP Solver: solver parameters, line search methods and QP solver selection

• MPC: using SSQP in a receding-horizon control loop

• Examples: full trajectory generation and MPC examples for real robot models