OpenSCvx

https://openscvx.github.io/OpenSCvx/
Admin
OpenSCvx is an open-source Python library for nonlinear trajectory optimization that turns nonlinear...
Tokens:1,715,004
Snippets:6,931
Trust Score:9.7
Update:1 month ago
Show doc for...
Tokens:
Raw
### 3DoF PDG Realtime Visualization Setup

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/realtime/3DoF_pdg_realtime?q=

Sets up a viser scene for a real-time 3DoF PDG example. Adds a trajectory point cloud, start and target spheres, transform controls for dragging, and a glideslope cone mesh. Also creates GUI folders for optimization metrics, problem control, and algorithm weights with update callbacks.

```Python
trajectory_handle = server.scene.add_point_cloud(
    "/trajectory",
    points=np.zeros((1, 3), dtype=np.float32),
    colors=(255, 255, 0),
    point_size=30.0 * VISER_SCENE_SCALE,
)
start_handle = server.scene.add_icosphere(
    "/start",
    radius=40.0 * VISER_SCENE_SCALE,
    color=(100, 255, 100),
    position=tuple(np.asarray(pdg.position.initial, dtype=np.float64) * VISER_SCENE_SCALE),
)
target_handle = server.scene.add_icosphere(
    "/target",
    radius=40.0 * VISER_SCENE_SCALE,
    color=(255, 100, 100),
    position=(
        float(pdg.final_position.value[0]) * VISER_SCENE_SCALE,
        float(pdg.final_position.value[1]) * VISER_SCENE_SCALE,
        0.0,
    ),
)
start_drag = server.scene.add_transform_controls(
    "/start_drag",
    position=tuple(np.asarray(pdg.position.initial, dtype=np.float64) * VISER_SCENE_SCALE),
    scale=40.0 * VISER_SCENE_SCALE,
    disable_rotations=True,
    visible=True,
)
target_drag = server.scene.add_transform_controls(
    "/target_drag",
    position=(
        float(pdg.final_position.value[0]) * VISER_SCENE_SCALE,
        float(pdg.final_position.value[1]) * VISER_SCENE_SCALE,
        0.0,
    ),
    scale=40.0 * VISER_SCENE_SCALE,
    disable_rotations=True,
    visible=True,
)

def _cone_height_scaled() -> float:
    init_z = float(np.asarray(pdg.initial_position.value, dtype=np.float64)[2])
    return max(abs(init_z), 100.0) * VISER_SCENE_SCALE

def _update_glideslope_cone() -> None:
    apex = np.array(
        [
            float(pdg.final_position.value[0]) * VISER_SCENE_SCALE,
            float(pdg.final_position.value[1]) * VISER_SCENE_SCALE,
            0.0,
        ],
        dtype=np.float32,
    )
    cone_vertices, cone_faces = _generate_cone_mesh(
        apex=apex,
        height=_cone_height_scaled(),
        half_angle_deg=_to_deg(float(pdg.glideslope_angle.value)),
        n_segments=48,
    )
    glideslope_cone_handle.vertices = cone_vertices
    glideslope_cone_handle.faces = cone_faces

cone_vertices, cone_faces = _generate_cone_mesh(
    apex=np.array(
        [
            float(pdg.final_position.value[0]) * VISER_SCENE_SCALE,
            float(pdg.final_position.value[1]) * VISER_SCENE_SCALE,
            0.0,
        ],
        dtype=np.float32,
    ),
    height=_cone_height_scaled(),
    half_angle_deg=_to_deg(float(pdg.glideslope_angle.value)),
    n_segments=48,
)
glideslope_cone_handle = server.scene.add_mesh_simple(
    "/constraints/glideslope_cone",
    vertices=cone_vertices,
    faces=cone_faces,
    color=(80, 200, 120),
    wireframe=False,
    opacity=0.2,
)

state = {"running": True, "reset_requested": False}

with server.gui.add_folder("Optimization Metrics"):
    metrics_text = server.gui.add_markdown(
        format_metrics_markdown(
            {
                "iter": 0,
                "J_tr": 0.0,
                "J_vb": 0.0,
                "J_vc": 0.0,
                "cost": 0.0,
                "dis_time": 0.0,
                "solve_time": 0.0,
                "prob_stat": "--",
            }
        )
    )

with server.gui.add_folder("Problem Control", expand_by_default=True):
    reset_button = server.gui.add_button("Apply Changes + Reset Problem")

    @reset_button.on_click
    def _(_) -> None:
        state["reset_requested"] = True

with server.gui.add_folder("Algorithm Weights"):
    lam_cost = server.gui.add_number(
        "lam_cost",
        initial_value=optimization_problem.algorithm.lam_cost,
        min=1e-8,
        max=1e5,
        step=0.01,
    )
    lam_vc = server.gui.add_number(
        "lam_vc",
        initial_value=optimization_problem.algorithm.lam_vc,
        min=1e-8,
        max=1e5,
        step=0.01,
    )
    lam_prox = server.gui.add_number(
        "lam_prox",
        initial_value=optimization_problem.algorithm.lam_prox,
        min=1e-8,
        max=1e5,
        step=0.01,
    )

    @lam_cost.on_update
    def _(_) -> None:
        optimization_problem.algorithm.lam_cost = float(lam_cost.value)

    @lam_vc.on_update
    def _(_) -> None:
        optimization_problem.algorithm.lam_vc = float(lam_vc.value)

    @lam_prox.on_update
    def _(_) -> None:
        optimization_problem.algorithm.lam_prox = float(lam_prox.value)
```

--------------------------------

### 7-DOF arm viewcone example: parameters and states

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/arm/7_dof_arm_vp

Initialization and parameter setup for the 7-DOF arm with viewcone constraints. Defines link lengths, joint inertias, camera cone parameters, screw axes for PoE kinematics, home configuration, and state bounds. Requires jaxlie via `pip install openscvx[lie]`.

```python
import os
import sys

import jax.numpy as jnp
import numpy as np

# Add grandparent directory to path to import examples.plotting
current_dir = os.path.dirname(os.path.abspath(__file__))
grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
sys.path.append(grandparent_dir)

import openscvx as ox
from openscvx.plotting import plot_controls, plot_scp_iterations

# =============================================================================
# Robot Parameters
# =============================================================================

N_JOINTS = 7

# Link lengths (meters)
d1 = 0.340  # Base height
a2 = 0.300  # Shoulder to elbow
a3 = 0.250  # Elbow to wrist
a4 = 0.150  # Wrist to end-effector

# Joint inertias (simplified, kg*m^2) — decreasing from base to tip
inertia = np.array([0.08, 0.06, 0.05, 0.04, 0.02, 0.01, 0.005])

# Number of discretization nodes
n = 5
total_time = 4.0

# =============================================================================
# Wrist Camera (Viewcone) Parameters
# =============================================================================
# Camera boresight aligns with EE body x-axis (arm extension direction).
# Cone defined by half-angles alpha_x, alpha_y in sensor frame.

alpha_x = 6.0  # Cone parameter: half-angle = pi/alpha_x = 60 deg
alpha_y = 6.0
A_cone = np.diag(
    [
        1 / np.tan(np.pi / alpha_x),
        1 / np.tan(np.pi / alpha_y),
        0,
    ]
)
c = jnp.array([0, 0, 1])  # Boresight in sensor frame
norm_type = 2
R_sb = jnp.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])  # Sensor-to-body rotation

# Viewpoint targets — workpiece features the wrist camera must track (centered around y=0)
vp_targets = np.array(
    [
        [0.25, 0.00, 0.10],
        [0.35, 0.00, 0.10],
        [0.30, 0.05, 0.05],
    ]
)

# =============================================================================
# Screw Axes for Product of Exponentials
# =============================================================================
# Alternating z-y rotation axes (iiwa/Panda-like layout).
# Home configuration: arm extended along +x at height d1.
#
# Each screw axis ξ = [v; ω] where ω is the rotation axis and v = -ω × q
# for a point q on the joint axis.

screw_axes = np.array(
    [
        # Joint 1: z-rotation at origin (base yaw)
        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
        # Joint 2: y-rotation at [0, 0, d1] (shoulder pitch)
        [-d1, 0.0, 0.0, 0.0, 1.0, 0.0],
        # Joint 3: z-rotation at [a2, 0, d1] (upper arm roll)
        [0.0, -a2, 0.0, 0.0, 0.0, 1.0],
        # Joint 4: y-rotation at [a2, 0, d1] (elbow pitch)
        [-d1, 0.0, a2, 0.0, 1.0, 0.0],
        # Joint 5: z-rotation at [a2+a3, 0, d1] (forearm roll)
        [0.0, -(a2 + a3), 0.0, 0.0, 0.0, 1.0],
        # Joint 6: y-rotation at [a2+a3, 0, d1] (wrist pitch)
        [-d1, 0.0, a2 + a3, 0.0, 1.0, 0.0],
        # Joint 7: z-rotation at [a2+a3+a4, 0, d1] (tool roll)
        [0.0, -(a2 + a3 + a4), 0.0, 0.0, 0.0, 1.0],
    ]
)

# Home configuration: EE at [a2+a3+a4, 0, d1] with identity rotation
T_home = np.array(
    [
        [1.0, 0.0, 0.0, a2 + a3 + a4],
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, 1.0, d1],
        [0.0, 0.0, 0.0, 1.0],
    ]
)

# =============================================================================
# States
# =============================================================================

# Joint angles (7,)
angle = ox.State("angle", shape=(N_JOINTS,))
angle.max = np.deg2rad([170, 120, 170, 120, 170, 120, 175])
angle.min = -angle.max
angle.initial = np.zeros(N_JOINTS)
angle.final = [("free", 0.0)] * N_JOINTS

# Joint velocities (7,)
velocity = ox.State("velocity", shape=(N_JOINTS,))
velocity.max = np.full(N_JOINTS, 3.0)
velocity.min = -velocity.max
velocity.initial = np.zeros(N_JOINTS)
velocity.final = np.zeros(N_JOINTS)

states = [angle, velocity]

# =============================================================================
# Controls
# =============================================================================

```

--------------------------------

### Obstacle Avoidance Example Setup

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/drone/obstacle_avoidance?q=

Sets up the quadrotor obstacle avoidance problem: imports, state/control definitions, dynamics, and obstacle constraints. Uses OpenSCVX and JAX. The file is examples/drone/obstacle_avoidance.py.

```python
import os
import sys

import jax.numpy as jnp
import numpy as np

# Add grandparent directory to path to import examples.plotting
current_dir = os.path.dirname(os.path.abspath(__file__))
grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
sys.path.append(grandparent_dir)

import openscvx as ox
from examples.plotting_viser import (
    create_animated_plotting_server,
    create_scp_animated_plotting_server,
)
from openscvx import Problem
from openscvx.utils import generate_orthogonal_unit_vectors

n = 6
total_time = 4.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, 10, 20])
position.min = np.array([-200.0, -100, 0])
position.initial = np.array([10.0, 0, 2])
position.final = [-10.0, 0, 2]

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)]

attitude = ox.State("attitude", shape=(4,))  # Quaternion [qw, qx, qy, qz]
attitude.max = np.array([1, 1, 1, 1])
attitude.min = np.array([-1, -1, -1, -1])
attitude.initial = [("free", 1.0), ("free", 0), ("free", 0), ("free", 0)]
attitude.final = [("free", 1.0), ("free", 0), ("free", 0), ("free", 0)]

angular_velocity = ox.State("angular_velocity", shape=(3,))  # Angular velocity [wx, wy, wz]
angular_velocity.max = np.array([10, 10, 10])
angular_velocity.min = np.array([-10, -10, -10])
angular_velocity.initial = [("free", 0), ("free", 0), ("free", 0)]
angular_velocity.final = [("free", 0), ("free", 0), ("free", 0)]

# Define control components
thrust_force = ox.Control("thrust_force", shape=(3,))  # Thrust forces [fx, fy, fz]
thrust_force.max = np.array([0, 0, 4.179446268 * 9.81])
thrust_force.min = np.array([0, 0, 0])
initial_control = np.array([0.0, 0.0, thrust_force.max[2]])
thrust_force.guess = np.repeat(np.expand_dims(initial_control, axis=0), n, axis=0)

torque = ox.Control("torque", shape=(3,))  # Control torques [tau_x, tau_y, tau_z]
torque.max = np.array([18.665, 18.665, 0.55562])
torque.min = np.array([-18.665, -18.665, -0.55562])
torque.guess = np.zeros((n, 3))


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


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

# Normalize quaternion for dynamics
q_norm = ox.linalg.Norm(attitude)
attitude_normalized = attitude / q_norm

# Define dynamics as dictionary mapping state names to their derivatives
J_b_inv = 1.0 / J_b
J_b_diag = ox.linalg.Diag(J_b)

dynamics = {
    "position": velocity,
    "velocity": (1.0 / m) * ox.spatial.QDCM(attitude_normalized) @ thrust_force
    + np.array([0, 0, g_const], dtype=np.float64),
    "attitude": 0.5 * ox.spatial.SSMP(angular_velocity) @ attitude_normalized,
    "angular_velocity": ox.linalg.Diag(J_b_inv)
    @ (torque - ox.spatial.SSM(angular_velocity) @ J_b_diag @ angular_velocity),
}


A_obs = []
radius = []
axes = []

# Default values for the obstacle centers
obstacle_center_positions = [
    np.array([-5.1, 0.1, 2]),
    np.array([0.1, 0.1, 2]),
    np.array([5.1, 0.1, 2]),
]

# Define obstacle centers as parameters for runtime updates
obstacle_centers = [
    ox.Parameter("obstacle_center_1", shape=(3,), value=obstacle_center_positions[0]),
    ox.Parameter("obstacle_center_2", shape=(3,), value=obstacle_center_positions[1]),
    ox.Parameter("obstacle_center_3", shape=(3,), value=obstacle_center_positions[2]),
]

np.random.seed(0)
for _ in obstacle_center_positions:
    ax = generate_orthogonal_unit_vectors()
    axes.append(generate_orthogonal_unit_vectors())
    rad = np.random.rand(3) + 0.1 * np.ones(3)
    radius.append(rad)
    A_obs.append(ax @ np.diag(rad**2) @ ax.T)

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

# Add obstacle constraints using symbolic expressions
for center, A in zip(obstacle_centers, A_obs):
    A_const = A

```

--------------------------------

### 3 Dof Arm Example (examples/arm/3_dof_arm.py)

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/arm/3_dof_arm

Full example for trajectory optimization of a 3-link arm using Product of Exponentials forward kinematics. It defines robot parameters, screw axes, states, controls, and forward kinematics using SE3Exp. Requires jaxlie: pip install openscvx[lie].

```python
import os
import sys

import numpy as np

# Add grandparent directory to path to import examples.plotting
current_dir = os.path.dirname(os.path.abspath(__file__))
grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
sys.path.append(grandparent_dir)

import openscvx as ox
from openscvx import Problem
from openscvx.plotting import plot_scp_convergence_histories, plot_scp_iterations

# =============================================================================
# Robot Parameters
# =============================================================================

# Link lengths (meters)
L1 = 0.4  # Base to shoulder
L2 = 0.3  # Shoulder to elbow
L3 = 0.2  # Elbow to end-effector

# Joint inertias (simplified, kg*m^2)
inertia = np.array([0.05, 0.03, 0.01])

# Number of discretization nodes
n = 3
total_time = 2.0

# =============================================================================
# Screw Axes for Product of Exponentials
# =============================================================================
# Each screw axis ξ = [v; ω] where:
#   ω = unit rotation axis
#   v = -ω × q (q = point on the axis)
#
# Robot configuration at home (q=0):
#   - Joint 1: z-rotation at origin
#   - Joint 2: y-rotation at height L1
#   - Joint 3: y-rotation at height L1, reach L2 in x

# Screw axes as rows of a 3x6 matrix
screw_axes = np.array(
    [
        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],  # Joint 1: z-rotation at origin
        [-L1, 0.0, 0.0, 0.0, 1.0, 0.0],  # Joint 2: y-rotation at [0, 0, L1]
        [-L1, 0.0, L2, 0.0, 1.0, 0.0],  # Joint 3: y-rotation at [L2, 0, L1]
    ]
)

# Home configuration: end-effector at [L2+L3, 0, L1] with identity rotation
T_home = np.array(
    [
        [1.0, 0.0, 0.0, L2 + L3],
        [0.0, 1.0, 0.0, 0.0],
        [0.0, 0.0, 1.0, L1],
        [0.0, 0.0, 0.0, 1.0],
    ]
)

# =============================================================================
# States
# =============================================================================

# Joint angles (3,)
angle = ox.State("angle", shape=(3,))
angle.max = np.array([np.pi, np.pi / 2, np.pi])
angle.min = np.array([-np.pi, -np.pi / 2, -np.pi])
angle.initial = np.array([0.0, 0.0, 0.0])
angle.final = [("free", 0.0), ("free", 0.0), ("free", 0.0)]

# Joint velocities (3,)
velocity = ox.State("velocity", shape=(3,))
velocity.max = np.array([5.0, 5.0, 5.0])
velocity.min = np.array([-5.0, -5.0, -5.0])
velocity.initial = np.array([0.0, 0.0, 0.0])
velocity.final = np.array([0.0, 0.0, 0.0])

states = [angle, velocity]

# =============================================================================
# Controls
# =============================================================================

# Joint torques (3,)
torque = ox.Control("torque", shape=(3,))
torque.max = np.array([10.0, 5.0, 2.0])
torque.min = np.array([-10.0, -5.0, -2.0])

controls = [torque]

# =============================================================================
# Forward Kinematics using Product of Exponentials
# =============================================================================
# T_ee(q) = exp(ξ₁q₁) @ exp(ξ₂q₂) @ exp(ξ₃q₃) @ T_home

# Symbolic screw axis expressions scaled by joint angles
xi_1 = ox.Constant(screw_axes[0])
xi_2 = ox.Constant(screw_axes[1])
xi_3 = ox.Constant(screw_axes[2])

twist_1 = xi_1 * angle[0]
twist_2 = xi_2 * angle[1]
twist_3 = xi_3 * angle[2]

# Exponential maps for each joint
T_01 = ox.lie.SE3Exp(twist_1)  # 4x4 transform
T_12 = ox.lie.SE3Exp(twist_2)
T_23 = ox.lie.SE3Exp(twist_3)

# Chain the transforms: T_ee = T_01 @ T_12 @ T_23 @ T_home
T_0_home = ox.Constant(T_home)
T_ee = T_01 @ T_12 @ T_23 @ T_0_home

# Extract end-effector position from homogeneous transform
p_ee = ox.Concat(T_ee[0, 3], T_ee[1, 3], T_ee[2, 3])

# =============================================================================
# Dynamics (simplified second-order)
# =============================================================================
# Using simplified dynamics: I * qdd = tau
#
# Note: Full manipulator dynamics M(q)q̈ + C(q,q̇)q̇ + G(q) = τ are not needed
# here. This example demonstrates the Lie algebra functionality (SE3Exp for
# Product of Exponentials FK), which is independent of the dynamics model.

I_inv = ox.Constant(1.0 / inertia)
```

--------------------------------

### Set up viser server and add visualization elements

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/arm/franka_fr3v2_viewplanning?q=

Creates a viser server, adds a grid and origin frame, and adds target markers, ghost trajectory, and animated trail for the end-effector. Requires viser and helper functions like add_target_markers, compute_velocity_colors, add_ghost_trajectory, add_animated_trail, and add_position_marker.

```python
    server = create_server(ee_pos, show_grid=False)
    server.scene.add_grid("/grid", width=1.5, height=1.5, cell_size=0.2)
    server.scene.add_frame("/origin", axes_length=0.08, axes_radius=0.003)

    tgt_arr = np.asarray(target.value).reshape(1, 3)
    add_target_markers(server, tgt_arr, radius=0.015, colors=[(255, 50, 50)])
    add_target_markers(
        server,
        vp_targets,
        radius=0.01,
        colors=[(50, 255, 50)] * len(vp_targets),
    )

    ee_colors = compute_velocity_colors(np.asarray(results.trajectory.get("velocity")))
    add_ghost_trajectory(server, ee_pos, ee_colors, point_size=0.005)
    _, update_trail = add_animated_trail(server, ee_pos, ee_colors, point_size=0.008)
    _, update_marker = add_position_marker(server, ee_pos, radius=0.012)
```

--------------------------------

### Main game loop: run()

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/mjx/triple_cartpole_game?q=

Sets up the Viser server, compiles MJX step functions for all levels if USE_MJX_PHYSICS is true, initializes game state, and adds persistent scene objects (grid, rail, stops). Exits if viser is not installed.

```python
def run() -> None:
    try:
        import viser
    except ImportError:
        sys.exit("viser not installed.  Run: pip install viser")

    # Pre-compile MJX step functions for all levels (only when MJX is active).
    _step_cache: dict[int, tuple] = {}  # n_links -> (step_fn, dt)
    if USE_MJX_PHYSICS:
        print("Compiling MJX physics (RK4 + mjx.forward) for all levels…")
        for _nl in range(1, N_LEVELS + 1):
            print(f"  Level {_nl} ({LEVEL_NAMES[_nl - 1]})…", end=" ", flush=True)
            _step_cache[_nl] = _make_mjx_step(_nl)
            print("done")
        print("Physics ready.  [MJX + RK4]\n")
    else:
        print("Physics backend: CPU MuJoCo mj_step (semi-implicit Euler)\n")

    server = viser.ViserServer()
    server.scene.set_up_direction("+z")

    # ── Game state ────────────────────────────────────────────────────────────
    gs: dict = {
        "level": 0,
        "status": "idle",
        "level_start": 0.0,
        "win_at": 0.0,
        "win_elapsed": 0.0,
        "balance_start": None,
        "advancing": False,
    }
    _sim: list[_Sim | None] = [None]
    _level_vis: list[dict | None] = [None]
    _handles: list[list] = [[]]  # all per-level scene handles
    _gen: list[int] = [0]  # incremented on each load; guards stale gizmo callbacks

    # ── Persistent scene ──────────────────────────────────────────────────────
    server.scene.add_grid(
        "/ground", width=10.0, height=4.0, cell_size=0.5, position=(0.0, 0.0, -0.115)
    )
    server.scene.add_box(
        "/rail",
        dimensions=(2 * RAIL_LIMIT + 0.1, 0.04, 0.015),
        position=(0.0, 0.0, 0.0),
        color=(130, 130, 130),
    )
    for sign in (-1, 1):
        server.scene.add_box(
            f"/rail/stop{'L' if sign < 0 else 'R'}",
            dimensions=(0.04, 0.12, 0.12),
            position=(sign * RAIL_LIMIT, 0.0, 0.0),
            color=(180, 60, 60),
        )
```

--------------------------------

### ik_interpolation usage example

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/init/inverse_kinematics

Initializes a 7-DOF arm trajectory reaching for a target using openscvx.init.ik_interpolation. The example shows the required arguments: keyframes as (position, quaternion_wxyz) tuples, node indices, screw axes, and home transform. Requires jaxlie, installable with 'pip install openscvx[lie]'.

```python
import openscvx as ox

angle.guess = ox.init.ik_interpolation(
    keyframes=[
        ([0.7, 0, 0.34], [1, 0, 0, 0]),  # home pose
        ([0.3, 0.3, 0.5], [1, 0, 0, 0]),  # target pose
    ],
    nodes=[0, 49],
    screw_axes=screw_axes,
    T_home=T_home,
)
```

--------------------------------

### Main entry point

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/realtime/3DoF_pdg_realtime?q=

Entry point that prints startup messages, creates the realtime server, and blocks forever.

```python
if __name__ == "__main__":
    print("Starting 3DoF PDG realtime optimization.")
    print("Open the URL shown below in your browser.\n")
    server = create_realtime_server(pdg.problem)
    server.sleep_forever()
```

--------------------------------

### Import dependencies and setup path for openscvx

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/mpc/double_integrator_discrete?q=

Imports required modules and adds the grandparent directory to the Python path to import openscvx. This setup is necessary for the example to run.

```python
import os
import sys

import numpy as np

# Add grandparent directory to path to import openscvx
current_dir = os.path.dirname(os.path.abspath(__file__))
grandparent_dir = os.path.dirname(os.path.dirname(current_dir))
sys.path.append(grandparent_dir)

import openscvx as ox
from openscvx import Problem
from openscvx.plotting import plot_scp_iterations, plot_states
from openscvx.plotting.viser import (
    add_animated_trail,
    add_animation_controls,
    add_ghost_trajectory,
    add_position_marker,
    compute_velocity_colors,
    create_server,
)
```

--------------------------------

### Example: Getting citations for an algorithm

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/algorithms/base

Shows how to retrieve and print BibTeX citations for an algorithm instance. The example creates a PenalizedTrustRegion object and iterates over its citation() method to print each BibTeX entry.

```python
algorithm = PenalizedTrustRegion()
for bibtex in algorithm.citation():
    print(bibtex)
```

--------------------------------

### Problem initialization: profiling, compilation, and solver setup

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/problem?q=

Initializes a profiling session if enabled, compiles dynamics and constraints with JAX JIT, generates discretization and propagation solvers, initializes the convex subproblem, prints a summary, and compiles solvers with caching. Uses symbolic AST hashing for cache file paths and handles impulsive discretization solvers.

```python
# Create a new profiling session (shared across initialize/solve/post_process)
        self._profiling_session = (
            profiling._create_session() if self.settings.dev.profiling else None
        )
        pr = profiling.profiling_start(self.settings.dev.profiling, self._profiling_session)

        t_0_while = time.time()
        # Ensure scaling matrices are correct
        self.settings.sim.__post_init__()

        # Create compiled (vmapped) propagation dynamics
        # This preserves the original un-vmapped versions in _lowered
        self._compiled_dynamics_prop = Dynamics(
            f=jax.vmap(self._lowered.dynamics_prop.f, in_axes=(0, 0, 0, None)),
        )

        # Create compiled (JIT-compiled) constraints as new instances
        # This preserves the original un-JIT'd versions in _lowered
        # TODO: (haynec) switch to AOT instead of JIT
        compiled_nodal = [
            LoweredNodalConstraint(
                func=jax.jit(c.func),
                grad_g_x=jax.jit(c.grad_g_x),
                grad_g_u=jax.jit(c.grad_g_u),
                nodes=c.nodes,
            )
            for c in self._lowered.jax_constraints.nodal
        ]

        compiled_cross_node = [
            LoweredCrossNodeConstraint(
                func=jax.jit(c.func),
                grad_g_X=jax.jit(c.grad_g_X),
                grad_g_U=jax.jit(c.grad_g_U),
            )
            for c in self._lowered.jax_constraints.cross_node
        ]

        self._compiled_constraints = LoweredJaxConstraints(
            nodal=compiled_nodal,
            cross_node=compiled_cross_node,
            ctcs=self._lowered.jax_constraints.ctcs,  # CTCS aren't JIT-compiled here
        )

        # Generate discretization solver via the discretizer (handles Jacobians + vmapping)
        self._discretization_solver = self._discretizer.get_solver(
            self._lowered.dynamics, self.settings
        )
        self._discretization_solver_impulsive = get_impulsive_discretization_solver(
            self._lowered.dynamics_discrete
        )
        self._propagation_solver = get_propagation_solver(
            self._compiled_dynamics_prop.f, self.settings, self._discretizer
        )

        # Build convex subproblem (solver was created in __init__, variables in lower)
        self._solver.initialize(self._lowered, self.settings)

        # Print problem summary (after solver is initialized so we can access problem stats)
        printing.print_problem_summary(
            self.settings,
            self._lowered,
            self._solver,
            self._algorithm,
            self._discretizer,
        )

        # Get cache file paths using symbolic AST hashing
        # This is more stable than hashing lowered JAX code
        dis_solver_file, prop_solver_file = get_solver_cache_paths(
            self.symbolic,
            dt=self.settings.prp.dt,
            total_time=self.settings.sim.total_time,
            byof=self._byof,
        )

        # Compile the discretization solver
        self._discretization_solver = load_or_compile_discretization_solver(
            self._discretization_solver,
            dis_solver_file,
            self._parameters,  # Plain dict for JAX
            self.settings.sim.n,
            self.settings.sim.n_states,
            self.settings.sim.n_controls,
            save_compiled=self.settings.sim.save_compiled,
            debug=self.settings.dev.debug,
        )

        # Compile the impulsive/discrete discretization solver with the same pipeline.
        # This solver is evaluated on node-wise inputs (x_nodes, u_nodes), shape (N, ...).
        # if has_impulsive and self._discretization_solver_impulsive is not None:
        dis_imp_solver_file = dis_solver_file.with_name(
            f"{dis_solver_file.stem}_impulsive{dis_solver_file.suffix}"
        )
        self._discretization_solver_impulsive = load_or_compile_discretization_solver(
            self._discretization_solver_impulsive,
            dis_imp_solver_file,
            self._parameters,  # Plain dict for JAX
            self.settings.sim.n,
            self.settings.sim.n_states,
            self.settings.sim.n_controls,
            save_compiled=self.settings.sim.save_compiled,
            debug=self.settings.dev.debug,
            name="discrete",
        )

        # Setup propagation solver parameters
        dtau = 1.0 / (self.settings.sim.n - 1)
        dt_max = self.settings.sim.u.max[self.settings.sim.time_dilation_slice][0] * dtau
        self.settings.prp.max_tau_len = int(dt_max / self.settings.prp.dt) + 2
```

--------------------------------

### UnifiedControl Import Example

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/lowered?q=

Shows the initial import required to start creating a unified control from multiple Control objects.

```python
from openscvx.symbolic.unified import unify_controls
```

--------------------------------

### Setup Problem and Solver

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/arm/3_dof_arm?q=

Configures the optimization problem with time, dynamics, constraints, and solver settings.

```python
time = ox.Time(
    initial=0.0,
    final=total_time,
    min=0.0,
    max=total_time,
)

problem = Problem(
    dynamics=dynamics,
    states=states,
    controls=controls,
    time=time,
    constraints=constraints,
    N=n,
    algorithm={"lam_vb": 1e0},
)

# Solver settings
problem.settings.prp.dt = 0.01
```

--------------------------------

### Get initial state boundary conditions

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/symbolic/expr/state?q=

Example from the `initial` property docstring showing how to get initial state boundary conditions. Creates a State with shape (2,), sets initial values with a free boundary condition, and prints the values and types.

```python
x = State("x", (2,))
x.initial = [0, ("free", 1)]
print(x.initial)  # [0. 1.]
print(x.initial_type)  # ['Fix' 'Free']
```

--------------------------------

### Example: load_mjmodel usage

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/integrations/menagerie?q=

Shows how to load a model, disable contact flags, and convert to an MJX model. Requires mujoco and openscvx installed.

```python
import mujoco.mjx as mjx
from openscvx.integrations.menagerie import load_mjmodel

mj_model = load_mjmodel("skydio_x2")
mj_model.opt.disableflags |= mujoco.mjtDisableBit.mjDSBL_CONTACT
mjx_model = mjx.put_model(mj_model)

```

--------------------------------

### 3D Visualization with viser (create_server, add_gates)

Source: https://openscvx.github.io/OpenSCvx/latest/Reference/plotting?q=

Shows how to create a viser server, add gates, and run the server loop for interactive 3D visualization.

```python
from openscvx.plotting import viser
    server = viser.create_server(positions)
    viser.add_gates(server, gate_vertices)
    server.sleep_forever()
```

--------------------------------

### Obstacle Avoidance Realtime GUI Setup and Callbacks

Source: https://openscvx.github.io/OpenSCvx/latest/Examples/realtime/obstacle_avoidance_realtime?q=

Sets up GUI controls for optimization metrics, weights, problem reset, and obstacle positions. Includes callbacks for updating parameters and syncing GUI inputs with 3D drag handles.

```python
    state = {
        "running": True,
        "reset_requested": False,
    }

    # =========================================================================
    # GUI Controls
    # =========================================================================

    # --- Optimization Metrics ---
    with server.gui.add_folder("Optimization Metrics"):
        metrics_text = server.gui.add_markdown(
            """**Iteration:** 0
**J_tr:** 0.00E+00
**J_vb:** 0.00E+00
**J_vc:** 0.00E+00
**Objective:** 0.00E+00
**Dis Time:** 0.0ms
**Solve Time:** 0.0ms
**Status:** --"""
        )

    # --- Optimization Weights ---
    with server.gui.add_folder("Optimization Weights"):
        lam_cost_input = server.gui.add_number(
            "λ_cost",
            initial_value=optimization_problem.algorithm.lam_cost,
            min=1e-6,
            max=1e6,
            step=0.01,
        )
        lam_tr_input = server.gui.add_number(
            "λ_tr (lam_prox)",
            initial_value=optimization_problem.algorithm.lam_prox,
            min=1e-6,
            max=1e6,
            step=0.1,
        )

        @lam_cost_input.on_update
        def _(_) -> None:
            optimization_problem.algorithm.lam_cost = lam_cost_input.value

        @lam_tr_input.on_update
        def _(_) -> None:
            optimization_problem.algorithm.lam_prox = lam_tr_input.value

    # --- Problem Control ---
    with server.gui.add_folder("Problem Control"):
        reset_button = server.gui.add_button("Reset Problem")

        @reset_button.on_click
        def _(_) -> None:
            state["reset_requested"] = True
            print("Problem reset requested")

    # --- Obstacle Controls ---
    obstacle_vector_inputs = []
    with server.gui.add_folder("Obstacle Positions", expand_by_default=False):
        server.gui.add_markdown("*Click an obstacle in 3D view to select and drag it*")

        reset_obstacles_button = server.gui.add_button("Reset All Obstacles")

        @reset_obstacles_button.on_click
        def _(_) -> None:
            # Deselect any selected obstacle
            select_obstacle(None)
            for i, vec_input in enumerate(obstacle_vector_inputs):
                original = initial_centers[i]
                vec_input.value = tuple(original)
                obstacle_params[i].value = np.array(original)
                param_name = f"obstacle_center_{i + 1}"
                optimization_problem.parameters[param_name] = np.array(original)
                # Update drag handle and obstacle positions
                obstacle_drag_handles[i].position = tuple(original)
                obstacle_handles[i].position = tuple(original)
            print("Obstacles reset to initial positions")

        for i in range(n_obstacles):
            initial_pos = obstacle_params[i].value
            vec_input = server.gui.add_vector3(
                f"Obstacle {i + 1}",
                initial_value=tuple(initial_pos),
                step=0.5,
            )
            obstacle_vector_inputs.append(vec_input)

            # Callback for GUI vector3 input -> update params and scene objects
            def make_obstacle_gui_callback(obs_idx: int, input_handle):
                @input_handle.on_update
                def _(_) -> None:
                    new_center = np.array(input_handle.value)
                    obstacle_params[obs_idx].value = new_center
                    param_name = f"obstacle_center_{obs_idx + 1}"
                    optimization_problem.parameters[param_name] = new_center
                    # Sync drag handle and obstacle positions
                    obstacle_drag_handles[obs_idx].position = tuple(new_center)
                    obstacle_handles[obs_idx].position = tuple(new_center)

                return _

            make_obstacle_gui_callback(i, vec_input)

    # Wire up drag handle callbacks (must be done after obstacle_vector_inputs is populated)
    def make_drag_callback(obs_idx: int, drag_handle):
        @drag_handle.on_update
        def _(_) -> None:
            new_center = np.array(drag_handle.position)
            obstacle_params[obs_idx].value = new_center
            param_name = f"obstacle_center_{obs_idx + 1}"
            optimization_problem.parameters[param_name] = new_center
            # Sync GUI vector3 input and obstacle position
            obstacle_vector_inputs[obs_idx].value = tuple(new_center)
            obstacle_handles[obs_idx].position = tuple(new_center)

        return _

    for i in range(n_obstacles):
        make_drag_callback(i, obstacle_drag_handles[i])

    # =========================================================================
    # Helper Functions
    # =========================================================================

    def update_metrics(results: dict) -> None:
        """Update the metrics markdown display."""
        metrics_text.content = format_metrics_markdown(results)

```
Tokens:
Raw