Panda Circle (Trajectory Generation)¶

examples/trajgen/python/panda/panda_circle.py generates a trajectory that drives a Franka Panda arm's end effector around a circle on the yz-plane, optionally constrained to stay within a square window inscribed in that circle. Where Pendubot tracked a fixed target configuration, this example tracks a moving target pose in task space: at every stage, the cost doesn't care what the joint angles are, only where the end effector ends up and how it's oriented, which is a much more common way to specify a manipulation task. It's the best next step after Pendubot: frame-relative pose tracking, a real (non-XML-derived) box constraint, and InverseDynamics mode.

Setup and the target trajectory¶

examples/trajgen/python/panda/panda_circle.py

import ssqpy
import numpy as np
import argparse
from math import pi, sin, cos

"""
Panda Circle example
Perform a circular motion with the Panda's end effector on the yz-plane
Optionally, constrain the movement within a rectangle inside the circle
"""


def parse_args():
    parser = argparse.ArgumentParser(
        description="Panda end effector circle trajectory generation"
    )

    parser.add_argument(
        "-p",
        "--plot",
        action=argparse.BooleanOptionalAction,
        default=True,
        help="Whether to plot error and trajectory",
    )

    parser.add_argument(
        "-c",
        "--constrain",
        action=argparse.BooleanOptionalAction,
        default=True,
        help="Whether to add a box constraint within the circle for the end \
            effector",
    )

    parser.add_argument(
        "-o",
        "--output",
        default="../../data/panda_circle.json",
        help="Path of the output JSON file",
    )

    return parser.parse_args()


args = parse_args()

PLOT = args.plot
CONSTRAIN = args.constrain

if PLOT:
    import matplotlib.pyplot as plt

H = 400
dt = 0.01

# Weights
T_WGT = 3.0
R_WGT = 1.0
V_WGT = 3e-3
U_WGT = 3e-3

# Horizontal and vertical line coordinates for constraints
constraint_hlines = [0.1, 0.41]
constraint_vlines = [-0.15, 0.15]

# Circle properties
cx = 0.3
cy = 0
cz = 0.26
cr = 0.22

DISTANCE = 2 * pi
ω = DISTANCE / (H * dt)
φ0 = 0

# Trajectory
traj = []

# Keep x-coordinate constant
# Move on a circular path on the yz-plane
for k in range(H + 1):
    φk = φ0 + ω * k * dt
    traj.append(np.array([cx, cy + cr * sin(φk), cz + cr * cos(φk)]))

Rd = np.eye(3)
Rd[1, 1] = -1
Rd[2, 2] = -1

The circular path is precomputed as a plain Python list of 3D points, one per stage, parameterized by angular velocity \(\omega\) so the end effector completes exactly one full circle over the horizon (\(H = 400\) stages at \(dt = 0.01\text{s}\), i.e. four seconds). This is the target the cost will track, not a constraint: nothing stops the solver from cutting corners if doing so is cheaper, which matters once --constrain clips the circle into a square later on. Rd is the desired end-effector orientation (constant throughout, on the yz-plane the arm reaches "forward" with a fixed wrist rotation), expressed as a rotation matrix rather than e.g. Euler angles, since that's what the frame-pose costs expect and it avoids the singularities/ambiguities of angle-based representations.

The two command line flags, --plot and --constrain (both on by default, and turned off with --no-plot and --no-constrain), are the two things worth toggling first if you want to see this example do something different before reading further: --plot shows the tracking error and the resulting trajectory after solving; --constrain decides whether the box constraint (below) is actually applied.

Building the model¶

examples/trajgen/python/panda/panda_circle.py

# Create an SSQP model with H and dt, from the URDF at the below path
model = ssqpy.model.Model(
    H,
    dt,
    from_preset=ssqpy.model.Preset.Panda,
    solver_mode=ssqpy.model.SolverMode.InverseDynamics,
)

nq = model.getnq()
nv = model.getnv()
nu = model.getnu()

# Use the following initial state
# This will act as:
#  1) An initial state for the system at the first time step (x0); this cannot
#     be changed by the solver
#  2) An initial guess for every time step xk; this will be changed by the
#     solver to minimize the cost and satisfy the constraints
# If we wanted to help the solver converge faster, we could also choose to
# provide a more accurate initial guess
init_state = np.zeros(nq + nv)
init_state[:nq] = [
    0.0013,
    -0.9115,
    -0.0010,
    -2.6316,
    -0.0008,
    1.7431,
    0.7862,
    0.0,
    0.0,
]

This model uses InverseDynamics solver mode, so controls are joint accelerations rather than torques, which the example's comments note can simplify convergence for this kind of task: accelerations enter the dynamics linearly, so the QP the solver builds at each iteration is a better local approximation of the true problem than it would be with torques, where the mapping to acceleration is nonlinear in the joint configuration.

init_state is a single, fixed arm configuration (not the resting/neutral one) chosen to already be reasonably close to the circle. As the comment in the source explains, this array does double duty: it's the fixed initial state \(\boldsymbol{x}_0\) (the arm has to actually start there), and the initial guess used to seed every other stage before the solver improves on it. A better initial guess generally means fewer SSQP iterations to converge; picking one near the expected solution, as here, is a common and effective way to help a local solver like SSQP. See finalize arguments in Model.

A real constraint: keeping the end effector inside a box¶

examples/trajgen/python/panda/panda_circle.py

# Create a box constraint for the position of the end effector
# This will constrain the position within `vlines[0]` <= p[1] <= `vlines[1]` in the
# y-axis and `hlines[0]` <= p[2] <= `hlines[1]` in the z-axis
# Note: here, we only create the constraint; for it to be applied to some stage
# of the horizon, we actually have to add it to the model
lb = np.array((-np.inf, constraint_vlines[0], constraint_hlines[0]))
ub = np.array((np.inf, constraint_vlines[1], constraint_hlines[1]))
box_constraint = ssqpy.model.constraints.FrameTranslationBound(
    model, "panda_ee", lb, ub
)

FrameTranslationBound is a predefined constraint bounding a frame's position component-wise; -np.inf/np.inf leaves the x-coordinate free (the plane the circle lives on is already fixed by the cost, so the constraint only needs to act on y/z) while the y/z coordinates are boxed in around the circle. Since the box is inscribed inside the circle, this constraint is genuinely active: whenever the tracked point on the circle falls outside the box (near the four "corners" of the circle relative to the square), the solver has to trade off cost against constraint satisfaction, and the resulting path visibly flattens against the box edges instead of following the circle exactly. This is a good example to compare --constrain against --no-constrain and look at the --plot output for; with it off, the trajectory follows the circle everywhere, since nothing stops it from doing so.

The constraint object is created once and reused across every stage it's added to, since constraints don't need to be re-instantiated per stage unless their bounds actually differ.

Per-stage costs and constraints¶

examples/trajgen/python/panda/panda_circle.py

for k in range(H):
    # Add a squared pose error cost for the end effector (`"panda_ee"` frame) to
    # follow the generated trajectory, while keeping its orientation at `Rd`
    # We use weights `P_WGT` and `R_WGT` for the translation and rotation
    # respectively
    model.addCost(
        k,
        ssqpy.model.costs.FrameSquaredPoseErrorCost(
            model, "panda_ee", traj[k], Rd, T_WGT, R_WGT
        ),
    )

    # Add regularization costs for joint velocity (v) and control (u)
    model.addCost(k, ssqpy.model.costs.SquaredJointVelocityCost(model, V_WGT))
    model.addCost(k, ssqpy.model.costs.SquaredControlCost(model, U_WGT))

    if CONSTRAIN:
        # Add the previously created box constraint to stage k
        model.addConstraints(k, box_constraint)

# Add the same pose error for the terminal stage
model.addCost(
    H,
    ssqpy.model.costs.FrameSquaredPoseErrorCost(
        model, "panda_ee", traj[H], Rd, T_WGT, R_WGT
    ),
)

# There are no controls (u) at the terminal stage, so we cannot add a control
# regularization cost; thus, we only add the joint velocity regularization cost
model.addCost(H, ssqpy.model.costs.SquaredJointVelocityCost(model, V_WGT))

if CONSTRAIN:
    # Add box constraint to terminal stage as well
    model.addConstraints(H, box_constraint)

FrameSquaredPoseErrorCost is added at every stage with that stage's own target point from traj and the fixed orientation Rd, weighted separately for translation (T_WGT) and rotation (R_WGT); translation is weighted three times as heavily here, so the solver prioritizes tracing the circle's shape over holding the wrist exactly level, though both matter. Velocity and control regularization are added too, at every stage, for the same reason as in Pendubot: without them, the solver has no reason to prefer a smooth, low-effort trajectory among the many that satisfy the pose cost equally well. The box constraint is added too, if --constrain is set.

Note the terminal stage (H) only gets the velocity regularization, not the control one, since there are no controls at the terminal stage (see Horizon, states and controls in Model); it does still get its own pose-tracking cost, added separately just below this block, so the very last point on the circle is tracked too.

Solving¶

examples/trajgen/python/panda/panda_circle.py

# Finalize the model
# This will generate the final model, ready for use by SSQP for optimization
# By passing a single configuration as an init_state, this configuration will
# be applied to all stages (including as an initial stage)
model.finalize(init_states=init_state)

# Initialize an SSQP solver object with the created model
# Parameters are passed as keyword arguments
# ADMM-specific parameters start with "admm_"
solver = ssqpy.solvers.ssqp(
    model,
    loose_success=False,
    tolerance=1e-2,
    # line_search_method=ssqpy.solvers.LineSearchMethod.Merit,
    admm_abs_tolerance=1e-3,
    admm_rel_tolerance=1e-3,
    admm_adaptive_rho_tolerance=10,
    # admm_rho_update_iter_freq=10,
    # admm_max_iter=1000,
    # qp_solver_type=ssqpy.solvers.QPSolverType.HPIPM,
    hpipm_max_iter=250,
)

# Solve using the SSQP solver and the model we created
state, x, u = solver.solve()

# Write the solution to an output file
# The generated trajectory can be visualized using the `visualize.py` script
solver.write(args.output)

print(f"Solver state: {state}")

The solver parameters here are mostly tightened tolerances (admm_abs_tolerance/admm_rel_tolerance) rather than a different QP backend; this is a much less pathological problem than Pendubot's swing-up (there's no local-minimum trap to route around), so the default ADMM solver, given a reasonably good initial guess, is enough. After solving, model.forwardKinematics(q, "panda_ee") is used (in the plotting code that follows, not shown here) to recover the actual end-effector path from the solved joint trajectory, for comparison against the target traj, using the same Pinocchio-backed algorithm used internally by the frame-based costs and constraints; this is exactly how you'd check tracking accuracy for any frame-based task, not just in plotting code.

Run it (from examples/trajgen/python/panda/, with the environment set up per Installation):

python panda_circle.py

With --plot (the default), two figures appear after solving: end-effector tracking error over the horizon on the left (small and roughly constant with --no-constrain, spiking near the box edges with --constrain), and the traced-out trajectory against the target circle on the right, with the box drawn in when constrained. The solver also prints its final SolverState to the terminal; Success (or LooseSuccess) is what you should see for the default configuration.

Things to try¶

Toggle --constrain and compare both plots: the tracking-error spikes and the flattened corners in the trajectory plot are a direct, visual consequence of the constraint being active.
Loosen or tighten the box (constraint_hlines/constraint_vlines) and watch how much of the circle gets clipped.
Try solver_mode=ForwardDynamics instead of InverseDynamics (you'll need to drop the hpipm_max_iter override or retune it) and compare convergence; this is a good way to build intuition for when the choice of solver mode actually matters in practice.
Replace init_state with the zero/neutral configuration (drop init_states=init_state from model.finalize) and see how much slower or less reliable convergence becomes from a worse initial guess.