Dr Double Integrator

Simplified drone racing using double integrator dynamics.

This example demonstrates time-optimal racing through gates using simplified double integrator (point mass) dynamics instead of full 6-DOF dynamics. The problem includes:

• 3-DOF point mass dynamics (position and velocity only)
• Direct force control inputs (no attitude dynamics)
• Sequential gate passage constraints
• Minimal time objective
• Loop closure constraint

File: examples/drone/dr_double_integrator.py

import jax.numpy as jnp
import numpy as np

import openscvx as ox
from examples.plotting import plot_animation_double_integrator
from openscvx import Problem
from openscvx.utils import gen_vertices, rot

n = 22  # Number of Nodes
total_time = 24.0  # Total time for the simulation

# Define state components
position = ox.State("position", shape=(3,))  # 3D position [x, y, z]
position.max = np.array([200.0, 100, 50])
position.min = np.array([-200.0, -100, 15])
position.initial = np.array([10.0, 0, 20])
position.final = [10.0, 0, 20]
position.guess = np.linspace(position.initial, position.final, n)

velocity = ox.State("velocity", shape=(3,))  # 3D velocity [vx, vy, vz]
velocity.max = np.array([100, 100, 100])
velocity.min = np.array([-100, -100, -100])
velocity.initial = np.array([0, 0, 0])
velocity.final = [("free", 0), ("free", 0), ("free", 0)]
velocity.guess = np.linspace(velocity.initial, [0, 0, 0], n)

# Define control
force = ox.Control("force", shape=(3,))  # Control forces [fx, fy, fz]
f_max = 4.179446268 * 9.81
force.max = np.array([f_max, f_max, f_max])
force.min = np.array([-f_max, -f_max, -f_max])
initial_control = np.array([0.0, 0, 10])
force.guess = np.repeat(initial_control[np.newaxis, :], n, axis=0)

m = 1.0  # Mass of the drone
g_const = -9.18
J_b = jnp.array([1.0, 1.0, 1.0])  # Moment of Inertia of the drone


### Gate Parameters ###
n_gates = 10
gate_centers = [
    np.array([59.436, 0.000, 20.0000]),
    np.array([92.964, -23.750, 25.5240]),
    np.array([92.964, -29.274, 20.0000]),
    np.array([92.964, -23.750, 20.0000]),
    np.array([130.150, -23.750, 20.0000]),
    np.array([152.400, -73.152, 20.0000]),
    np.array([92.964, -75.080, 20.0000]),
    np.array([92.964, -68.556, 20.0000]),
    np.array([59.436, -81.358, 20.0000]),
    np.array([22.250, -42.672, 20.0000]),
]

radii = np.array([2.5, 1e-4, 2.5])
A_gate = rot @ np.diag(1 / radii) @ rot.T
A_gate_cen = []
for center in gate_centers:
    center[0] = center[0] + 2.5
    center[2] = center[2] + 2.5
    A_gate_cen.append(A_gate @ center)
nodes_per_gate = 2
gate_nodes = np.arange(nodes_per_gate, n, nodes_per_gate)
vertices = []
for center in gate_centers:
    vertices.append(gen_vertices(center, radii))
### End Gate Parameters ###

# Define list of all states (needed for Problem and constraints)
states = [position, velocity]
controls = [force]

# Generate box constraints for all states
constraint_exprs = []
for state in states:
    constraint_exprs.extend([ox.ctcs(state <= state.max), ox.ctcs(state.min <= state)])

# Add gate constraints
for node, cen in zip(gate_nodes, A_gate_cen):
    A_gate_const = A_gate
    c_const = cen
    gate_constraint = (
        (ox.linalg.Norm(A_gate_const @ position - c_const, ord="inf") <= np.array([1.0]))
        .convex()
        .at([node])
    )
    constraint_exprs.append(gate_constraint)


# Define dynamics as dictionary mapping state names to their derivatives
dynamics = {
    "position": velocity,
    "velocity": (1 / m) * force + np.array([0, 0, g_const], dtype=np.float64),
}


position_bar = np.linspace(position.initial, position.final, n)

i = 0
origins = [position.initial]
ends = []
for center in gate_centers:
    origins.append(center)
    ends.append(center)
ends.append(position.final)
gate_idx = 0
for _ in range(n_gates + 1):
    for k in range(n // (n_gates + 1)):
        position_bar[i] = origins[gate_idx] + (k / (n // (n_gates + 1))) * (
            ends[gate_idx] - origins[gate_idx]
        )
        i += 1
    gate_idx += 1

position.guess = position_bar

time = ox.Time(
    initial=0.0,
    final=("minimize", total_time),
    min=0.0,
    max=total_time,
)

problem = Problem(
    dynamics=dynamics,
    states=states,
    controls=controls,
    time=time,
    constraints=constraint_exprs,
    N=n,
)

problem.settings.prp.dt = 0.01
problem.settings.dis.custom_integrator = True

problem.settings.scp.w_tr = 2e0  # Weight on the Trust Reigon
problem.settings.scp.lam_cost = 1e-1  # 0e-1,  # Weight on the Minimal Time Objective
problem.settings.scp.lam_vc = (
    1e1  # 1e1,  # Weight on the Virtual Control Objective (not including CTCS Augmentation)
)
problem.settings.scp.ep_tr = 1e-3  # Trust Region Tolerance
problem.settings.scp.ep_vb = 1e-4  # Virtual Control Tolerance
problem.settings.scp.ep_vc = 1e-8  # Virtual Control Tolerance for CTCS
problem.settings.scp.cost_drop = 10  # SCP iteration to relax minimal final time objective
problem.settings.scp.cost_relax = 0.8  # Minimal Time Relaxation Factor
problem.settings.scp.w_tr_adapt = 1.4  # Trust Region Adaptation Factor
problem.settings.scp.w_tr_max_scaling_factor = 1e2  # Maximum Trust Region Weight

plotting_dict = {"vertices": vertices}

if __name__ == "__main__":
    problem.initialize()
    results = problem.solve()
    results = problem.post_process()

    results.update(plotting_dict)

    plot_animation_double_integrator(results, problem.settings).show()