openscvx 0.4.1.dev169__tar.gz → 0.4.1.dev171__tar.gz

{openscvx-0.4.1.dev169/openscvx.egg-info → openscvx-0.4.1.dev171}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.4.1.dev169
+Version: 0.4.1.dev171
 Summary: A general Python-based successive convexification implementation which uses a JAX backend.
 Author-email: Chris Hayner and Griffin Norris <haynec@uw.edu>
 License: Apache Software License

{openscvx-0.4.1.dev169 → openscvx-0.4.1.dev171}/docs/UsersGuide/07_lie.md

@@ -364,6 +364,98 @@ results = problem.post_process()
 
 After solving, `results.trajectory["T_ee"]` gives you the dense end-effector transform along the propagated trajectory, ready to feed straight into a viser animation or a downstream controller.
 
+## Extension: Self-Collision and Obstacle Avoidance
+
+The same intermediate `joint_transforms` we snapshotted above give us everything we need to constrain the *arm itself*, not just its end-effector. The [7-DOF arm with collisions](../Examples/arm/7_dof_arm_collision.md) example extends the pick-and-place setup with two extra constraints:
+
+- **Self-collision** between every pair of non-adjacent links.
+- **Obstacle avoidance** between every link and a spherical obstacle placed on the table.
+
+The idea is to approximate each link as a **capsule** (a sphere-swept line segment) between two consecutive keypoints, sample each capsule at a small grid of parameters $t \in [0, 1]$, and enforce a pairwise distance bound at every sample. `ox.Vmap` turns the inner loop into a single batched evaluation.
+
+First, lift each keypoint from its home position into the world frame using the transforms we already built. We pad with a trailing `1` so the $4 \times 4$ homogeneous transform acts on it correctly:
+
+```python
+keypoint_home_pos = {
+    "base":     np.array([0.0,          0.0, 0.0, 1.0]),
+    "shoulder": np.array([0.0,          0.0, d1,  1.0]),
+    "elbow":    np.array([a2,           0.0, d1,  1.0]),
+    "wrist":    np.array([a2 + a3,      0.0, d1,  1.0]),
+    "ee":       np.array([a2 + a3 + a4, 0.0, d1,  1.0]),
+}
+
+def _keypoint_world(name):
+    p_hom = joint_transforms[name] @ ox.Constant(keypoint_home_pos[name])
+    return ox.Concat(p_hom[0], p_hom[1], p_hom[2])
+
+kp = {name: _keypoint_world(name) for name in joint_names}
+```
+
+Then describe each link as `(start, end, radius)`:
+
+```python
+link_specs = [
+    ("base",     "shoulder", 0.06),
+    ("shoulder", "elbow",    0.05),
+    ("elbow",    "wrist",    0.04),
+    ("wrist",    "ee",       0.035),
+]
+```
+
+### Self-collision
+
+For every pair of non-adjacent capsules, sample both segments on a $t \times t$ grid and enforce that the sum of radii stays under the sampled distance:
+
+```python
+n_samples = 4
+ts = np.linspace(0.0, 1.0, n_samples)
+tt = np.stack(np.meshgrid(ts, ts, indexing="ij"), axis=-1).reshape(-1, 2)
+
+for i in range(len(link_specs)):
+    for j in range(i + 2, len(link_specs)):  # skip adjacent (shared joint)
+        a_start, a_end, r_a = link_specs[i]
+        b_start, b_end, r_b = link_specs[j]
+        pa0, pa1 = kp[a_start], kp[a_end]
+        pb0, pb1 = kp[b_start], kp[b_end]
+        r_sum = r_a + r_b
+
+        dists = ox.Vmap(
+            lambda t, pa0=pa0, pa1=pa1, pb0=pb0, pb1=pb1: ox.linalg.Norm(
+                ((1 - t[0]) * pa0 + t[0] * pa1)
+                - ((1 - t[1]) * pb0 + t[1] * pb1)
+            ),
+            batch=tt,
+        )
+        constraints.append(ox.ctcs(r_sum <= dists))
+```
+
+Skipping `j = i + 1` matters: adjacent capsules share a joint, so their minimum distance is identically zero and enforcing a strict capsule-capsule bound between them would be infeasible by construction.
+
+### Obstacle avoidance
+
+The same pattern handles a spherical obstacle — sample every link and keep it outside the inflated sphere:
+
+```python
+obstacle_center = np.array([0.35, 0.0, 0.25])
+obstacle_radius = 0.06
+
+for a_start, a_end, r_link in link_specs:
+    pa0, pa1 = kp[a_start], kp[a_end]
+    r_clear = obstacle_radius + r_link
+    obs_dists = ox.Vmap(
+        lambda t, pa0=pa0, pa1=pa1: ox.linalg.Norm(
+            ((1 - t) * pa0 + t * pa1) - ox.Constant(obstacle_center)
+        ),
+        batch=ts,
+    )
+    constraints.append(ox.ctcs(r_clear <= obs_dists))
+```
+
+Because each bound is wrapped in `ox.ctcs`, distances are enforced **continuously** along the trajectory — capsules cannot tunnel between nodes.
+
+!!! warning "Numerical performance"
+    Solve times on this example are currently noticeably slower than we would like when self-collision checking is included; expect a meaningful jump compared to the raw pick-and-place. We are actively working on it.
+
 ## Key Takeaways
 
 1. **Start simple with the minimal representation.** Joint angles + velocities give a pure ODE that drops cleanly into OpenSCvx's modeling stack — the easiest place to begin. Maximal coordinates are also a strong (and in some respects superior) choice for trajopt and will get their own tutorial in the future. The consequence of working in joint space is that task-space quantities are no longer states — they are symbolic functions of the state.
@@ -377,4 +469,5 @@ After solving, `results.trajectory["T_ee"]` gives you the dense end-effector tra
 - [API Reference: Lie algebra](../Reference/symbolic/expr/lie/index.md)
 - [Three-link arm Example](../Examples/arm/3_dof_arm.md)
 - [Seven-link arm Example](../Examples/arm/7_dof_arm.md)
+- [Seven-link arm with self-collision Example](../Examples/arm/7_dof_arm_collision.md)
 - [Seven-link arm with viewcone Example](../Examples/arm/7_dof_arm_vp.md)

openscvx-0.4.1.dev171/examples/arm/7_dof_arm_collision.py

@@ -0,0 +1,507 @@
+"""7-DOF redundant arm pick-and-place with Product of Exponentials FK.
+
+This example demonstrates trajectory optimization for a 7-DOF spatial arm
+performing a full pick-and-place motion: home -> pre-grasp -> grasp ->
+pre-place -> place. Uses Lie algebra operations for forward kinematics
+and IK-generated initial guesses.
+
+- 7 revolute joints with alternating z-y rotation axes
+- Product of Exponentials (PoE) forward kinematics using SE3Exp
+- Multi-waypoint pick-and-place trajectory
+- EE position constraints at each waypoint
+- Downward orientation constraints at grasp/place waypoints
+- IK-generated initial guess via damped least-squares
+- Joint torque control inputs
+
+The PoE formula computes forward kinematics as:
+    T_ee(q) = exp(ξ₁q₁) @ ... @ exp(ξ₇q₇) @ T_home
+
+Requires jaxlie: pip install openscvx[lie]
+"""
+
+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.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 (parameterized by segments between waypoints)
+nodes_per_segment = 2
+total_time = 4.0
+
+# =============================================================================
+# Pick-and-Place Waypoints
+# =============================================================================
+# Orientation: Ry(90°) points body +x toward world -z (EE facing down)
+q_down = np.array([np.cos(np.pi / 4), 0.0, np.sin(np.pi / 4), 0.0])
+q_identity = np.array([1.0, 0.0, 0.0, 0.0])
+
+pre_height = 0.15  # height above grasp/place positions
+
+grasp_pos = np.array([0.35, -0.25, 0.05])
+place_pos = np.array([0.35, 0.25, 0.05])
+home_pos = np.array([a2 + a3 + a4 - 0.01, 0.0, d1])
+pre_grasp_pos = grasp_pos + np.array([0.0, 0.0, pre_height])
+pre_place_pos = place_pos + np.array([0.0, 0.0, pre_height])
+
+waypoint_names = ["home", "pre_grasp", "grasp", "pre_grasp", "pre_place", "place"]
+waypoint_positions = [home_pos, pre_grasp_pos, grasp_pos, pre_grasp_pos, pre_place_pos, place_pos]
+waypoint_orientations = [q_identity, q_down, q_down, q_down, q_down, q_down]
+
+n_segments = len(waypoint_positions) - 1
+n = nodes_per_segment * n_segments + 1
+waypoint_nodes = [i * nodes_per_segment for i in range(len(waypoint_positions))]
+
+# =============================================================================
+# 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
+# =============================================================================
+
+# Joint torques (7,) — decreasing limits from base to tip
+torque = ox.Control("torque", shape=(N_JOINTS,))
+torque.max = np.array([8.0, 8.0, 4.0, 4.0, 2.0, 1.0, 0.5])
+torque.min = -torque.max
+
+controls = [torque]
+
+# =============================================================================
+# Forward Kinematics using Product of Exponentials
+# =============================================================================
+# T_ee(q) = exp(ξ₁q₁) @ ... @ exp(ξ₇q₇) @ T_home
+#
+# We build the FK chain incrementally so we can capture the intermediate
+# transforms at each keypoint (base, shoulder, elbow, wrist, ee) for
+# visualization via algebraic_prop.
+
+joint_names = ["base", "shoulder", "elbow", "wrist", "ee"]
+keypoint_after_joint = [0, 1, 2, 4, 7]  # how many joints precede each keypoint
+
+T_chain = ox.Constant(np.eye(4))
+joint_idx = 0
+joint_transforms = {}
+for name, n_joints in zip(joint_names, keypoint_after_joint):
+    while joint_idx < n_joints:
+        xi = ox.Constant(screw_axes[joint_idx])
+        T_chain = T_chain @ ox.lie.SE3Exp(xi * angle[joint_idx])
+        joint_idx += 1
+    joint_transforms[name] = T_chain
+
+T_ee = T_chain @ ox.Constant(T_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)
+
+dynamics = {
+    "angle": velocity,
+    "velocity": I_inv * torque,
+}
+
+# =============================================================================
+# Constraints
+# =============================================================================
+
+# Box constraints
+constraints = []
+for state in states:
+    constraints.extend(
+        [
+            ox.ctcs(state <= state.max),
+            ox.ctcs(state.min <= state),
+        ]
+    )
+
+# EE stays above the table
+constraints.append(ox.ctcs(p_ee[2] >= 0.0))
+
+# EE position constraint at each waypoint
+ee_tolerance = 0.01  # 1cm tolerance
+for i, (wp, node) in enumerate(zip(waypoint_positions, waypoint_nodes)):
+    wp_param = ox.Parameter(f"waypoint_{waypoint_names[i]}", shape=(3,), value=wp)
+    constraints.append((ox.linalg.Norm(p_ee - wp_param, ord=2) <= ee_tolerance).at([node]))
+
+# Downward orientation constraint at task waypoints (pre_grasp, grasp, pre_place, place)
+w, x, y, z = q_down
+R_des = np.array(
+    [
+        [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
+        [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
+        [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
+    ]
+)
+ori_error = ox.lie.SO3Log(R_des.T @ T_ee[:3, :3])  # (3,) rotation error vector
+ori_norm = ox.linalg.Norm(ori_error, ord=2)
+
+# Loose tolerance during transit, tight during grasp/place approach
+ori_tolerance = np.deg2rad(5.0)
+ori_tolerance_tight = np.deg2rad(0.1)
+
+# Tight: pre_grasp -> grasp -> pre_grasp
+constraints.append((ori_norm <= ori_tolerance_tight).over((waypoint_nodes[1], waypoint_nodes[3])))
+# Tight: pre_place -> place
+constraints.append((ori_norm <= ori_tolerance_tight).over((waypoint_nodes[4], waypoint_nodes[5])))
+
+# Self-collision: capsule-capsule distance between non-adjacent link pairs,
+# approximated by sampling points along each link and enforcing pairwise
+# sphere-swept distance >= sum of link radii.
+keypoint_home_pos = {
+    "base": np.array([0.0, 0.0, 0.0, 1.0]),
+    "shoulder": np.array([0.0, 0.0, d1, 1.0]),
+    "elbow": np.array([a2, 0.0, d1, 1.0]),
+    "wrist": np.array([a2 + a3, 0.0, d1, 1.0]),
+    "ee": np.array([a2 + a3 + a4, 0.0, d1, 1.0]),
+}
+
+
+def _keypoint_world(name):
+    p_hom = joint_transforms[name] @ ox.Constant(keypoint_home_pos[name])
+    return ox.Concat(p_hom[0], p_hom[1], p_hom[2])
+
+
+kp = {name: _keypoint_world(name) for name in joint_names}
+
+link_specs = [
+    ("base", "shoulder", 0.06),
+    ("shoulder", "elbow", 0.05),
+    ("elbow", "wrist", 0.04),
+    ("wrist", "ee", 0.035),
+]
+
+n_samples = 4
+ts = np.linspace(0.0, 1.0, n_samples)
+tt = np.stack(np.meshgrid(ts, ts, indexing="ij"), axis=-1).reshape(-1, 2)
+
+for i in range(len(link_specs)):
+    for j in range(i + 2, len(link_specs)):  # skip adjacent pairs (shared joint)
+        a_start, a_end, r_a = link_specs[i]
+        b_start, b_end, r_b = link_specs[j]
+        pa0, pa1 = kp[a_start], kp[a_end]
+        pb0, pb1 = kp[b_start], kp[b_end]
+        r_sum = r_a + r_b
+
+        dists = ox.Vmap(
+            lambda t, pa0=pa0, pa1=pa1, pb0=pb0, pb1=pb1: ox.linalg.Norm(
+                ((1 - t[0]) * pa0 + t[0] * pa1) - ((1 - t[1]) * pb0 + t[1] * pb1)
+            ),
+            batch=tt,
+        )
+        constraints.append(ox.ctcs(r_sum <= dists))
+
+# Spherical obstacle avoidance: sample each link and enforce distance from
+# obstacle center >= obstacle_radius + link_radius.
+obstacle_center = np.array([0.35, 0.0, 0.25])
+obstacle_radius = 0.06
+
+for a_start, a_end, r_link in link_specs:
+    pa0, pa1 = kp[a_start], kp[a_end]
+    r_clear = obstacle_radius + r_link
+    obs_dists = ox.Vmap(
+        lambda t, pa0=pa0, pa1=pa1: ox.linalg.Norm(
+            ((1 - t) * pa0 + t * pa1) - ox.Constant(obstacle_center)
+        ),
+        batch=ts,
+    )
+    constraints.append(ox.ctcs(r_clear <= obs_dists))
+
+# =============================================================================
+# Initial Guesses (via IK)
+# =============================================================================
+
+angle.guess = ox.init.ik_interpolation(
+    keyframes=list(zip(waypoint_positions, waypoint_orientations)),
+    nodes=waypoint_nodes,
+    screw_axes=screw_axes,
+    T_home=T_home,
+    angles_init=angle.initial,
+    angles_min=angle.min,
+    angles_max=angle.max,
+    sequential=True,
+)
+angle.initial = np.clip(angle.guess[0], angle.min, angle.max)
+velocity.guess = np.zeros((n, N_JOINTS))
+torque.guess = np.zeros((n, N_JOINTS))
+
+# =============================================================================
+# Problem Setup
+# =============================================================================
+
+time = ox.Time(
+    initial=0.0,
+    final=ox.Minimize(total_time),
+    min=0.0,
+    max=total_time,
+)
+
+problem = ox.Problem(
+    dynamics=dynamics,
+    states=states,
+    controls=controls,
+    time=time,
+    constraints=constraints,
+    N=n,
+    algorithm={"lam_vb": 1e1, "lam_vc": 1e2, "autotuner": ox.AugmentedLagrangian(eta_lambda=1e0)},
+    algebraic_prop={
+        "ee_position": p_ee,
+        **{f"T_{name}": T for name, T in joint_transforms.items()},
+    },
+)
+
+problem.settings.prp.dt = 0.01
+
+if __name__ == "__main__":
+    print("7-DOF Arm Pick-and-Place Trajectory Optimization with PoE FK")
+    print("=" * 60)
+    print(f"Link lengths: d1={d1}m, a2={a2}m, a3={a3}m, a4={a4}m")
+    print(f"Nodes: {n} ({nodes_per_segment} per segment, {n_segments} segments)")
+    for name, pos, node in zip(waypoint_names, waypoint_positions, waypoint_nodes):
+        print(f"  {name:>12s} (node {node:2d}): {list(pos)}")
+    print()
+
+    problem.initialize()
+    results = problem.solve()
+    results = problem.post_process()
+
+    # Extract results
+    final_q = results.trajectory["angle"][-1]
+    ee_pos = results.trajectory["ee_position"]  # (T, 3) from algebraic_prop
+
+    print()
+    print("Results:")
+    print(f"Final joint angles [deg]: {np.round(np.rad2deg(final_q), 1)}")
+    for name, pos, node in zip(waypoint_names, waypoint_positions, waypoint_nodes):
+        # Find the closest propagated frame to each waypoint node
+        t_node = node / (n - 1)
+        frame_idx = int(round(t_node * (len(ee_pos) - 1)))
+        ee_at_wp = ee_pos[frame_idx]
+        err = np.linalg.norm(ee_at_wp - pos)
+        print(
+            f"  {name:>12s}: pos=[{ee_at_wp[0]:.3f}, {ee_at_wp[1]:.3f}, {ee_at_wp[2]:.3f}]  "
+            f"err={err:.4f}m"
+        )
+
+    plot_scp_iterations(results).show()
+    plot_controls(results).show()
+
+    # =========================================================================
+    # Viser 3D Arm Animation
+    # =========================================================================
+
+    import jaxlie
+
+    from openscvx.plotting.viser import (
+        add_animated_trail,
+        add_animation_controls,
+        add_ellipsoid_obstacles,
+        add_ghost_trajectory,
+        add_position_marker,
+        add_target_markers,
+        compute_velocity_colors,
+        create_server,
+    )
+
+    # -- Extract joint keypoint positions from propagated transforms -------------
+    # Each T_{name} is (T, 4, 4) from algebraic_prop. Apply the keypoint's
+    # home position to get the world-frame position at each timestep.
+
+    keypoint_home_positions = {
+        "base": np.array([0.0, 0.0, 0.0]),
+        "shoulder": np.array([0.0, 0.0, d1]),
+        "elbow": np.array([a2, 0.0, d1]),
+        "wrist": np.array([a2 + a3, 0.0, d1]),
+        "ee": np.array([a2 + a3 + a4, 0.0, d1]),
+    }
+
+    n_frames = len(ee_pos)
+    keypoints = np.zeros((n_frames, 5, 3))
+    joint_quats = np.zeros((n_frames, 5, 4))
+    for k, name in enumerate(joint_names):
+        T_k = np.asarray(results.trajectory[f"T_{name}"])  # (T, 4, 4)
+        p_home = np.append(keypoint_home_positions[name], 1.0)
+        keypoints[:, k] = (T_k @ p_home)[:, :3]
+        joint_quats[:, k] = np.array(
+            [jaxlie.SO3.from_matrix(T_k[t, :3, :3]).wxyz for t in range(n_frames)]
+        )
+
+    # -- Create viser server ----------------------------------------------------
+
+    server = create_server(ee_pos, show_grid=False)
+    server.scene.add_grid("/grid", width=1.5, height=1.5, cell_size=0.25)
+    server.scene.add_frame("/origin", axes_length=0.1, axes_radius=0.003)
+
+    # Waypoint markers: grasp/place in red, pre-grasp/pre-place in orange, home in blue
+    marker_colors = {
+        "home": (100, 150, 255),
+        "pre_grasp": (255, 180, 50),
+        "grasp": (255, 50, 50),
+        "pre_place": (255, 180, 50),
+        "place": (255, 50, 50),
+    }
+    add_target_markers(
+        server,
+        waypoint_positions,
+        radius=0.015,
+        colors=[marker_colors[name] for name in waypoint_names],
+    )
+
+    add_ellipsoid_obstacles(
+        server,
+        centers=[obstacle_center],
+        radii=[np.full(3, 1.0 / obstacle_radius)],
+    )
+
+    # Ghost EE trajectory (faint full path)
+    ee_colors = compute_velocity_colors(np.asarray(results.trajectory.get("velocity")))
+    add_ghost_trajectory(server, ee_pos, ee_colors, point_size=0.005)
+
+    # Animated EE trail
+    _, update_trail = add_animated_trail(server, ee_pos, ee_colors, point_size=0.008)
+
+    # Animated EE position marker
+    _, update_marker = add_position_marker(server, ee_pos, radius=0.015)
+
+    # Animated arm links (line segments between consecutive keypoints)
+    # Per-segment colors: (N_segments, 2, 3) — same color at both endpoints
+    link_rgb = np.array(
+        [
+            [180, 180, 180],  # base -> shoulder
+            [100, 180, 255],  # shoulder -> elbow
+            [100, 255, 150],  # elbow -> wrist
+            [255, 200, 100],  # wrist -> ee
+        ],
+        dtype=np.uint8,
+    )
+    link_colors = np.stack([link_rgb, link_rgb], axis=1)  # (4, 2, 3)
+
+    # Initial arm segments: (4, 2, 3)
+    init_points = np.stack(
+        [np.stack([keypoints[0, k], keypoints[0, k + 1]]) for k in range(4)]
+    ).astype(np.float32)
+
+    arm_handle = server.scene.add_line_segments(
+        "/arm_links",
+        points=init_points,
+        colors=link_colors,
+        line_width=5.0,
+    )
+
+    # Joint coordinate frames
+    joint_frame_handles = []
+    for k in range(5):
+        h = server.scene.add_frame(
+            f"/joint_{joint_names[k]}",
+            wxyz=joint_quats[0, k].astype(np.float32),
+            position=keypoints[0, k].astype(np.float32),
+            axes_length=0.06,
+            axes_radius=0.002,
+        )
+        joint_frame_handles.append(h)
+
+    def update_arm(frame_idx: int) -> None:
+        pts = np.stack(
+            [np.stack([keypoints[frame_idx, k], keypoints[frame_idx, k + 1]]) for k in range(4)]
+        ).astype(np.float32)
+        arm_handle.points = pts
+        for k, h in enumerate(joint_frame_handles):
+            h.position = keypoints[frame_idx, k].astype(np.float32)
+            h.wxyz = joint_quats[frame_idx, k].astype(np.float32)
+
+    # Animation controls
+    traj_time = np.asarray(results.trajectory["time"])
+    add_animation_controls(
+        server,
+        traj_time,
+        [update_trail, update_marker, update_arm],
+        loop=True,
+    )
+
+    # Keep server running
+    server.sleep_forever()

{openscvx-0.4.1.dev169 → openscvx-0.4.1.dev171}/openscvx/_version.py

@@ -18,7 +18,7 @@ version_tuple: tuple[int | str, ...]
 commit_id: str | None
 __commit_id__: str | None
 
-__version__ = version = '0.4.1.dev169'
-__version_tuple__ = version_tuple = (0, 4, 1, 'dev169')
+__version__ = version = '0.4.1.dev171'
+__version_tuple__ = version_tuple = (0, 4, 1, 'dev171')
 
-__commit_id__ = commit_id = 'gbc266779e'
+__commit_id__ = commit_id = 'gc410cdf9e'

{openscvx-0.4.1.dev169 → openscvx-0.4.1.dev171}/openscvx/solvers/base.py

@@ -245,7 +245,14 @@ _SOLVER_MAP: Dict[str, type] = {}  # populated by __init__.py after all classes
 
 
 class SolverSpec(BaseModel):
-    """Validates solver configuration from dict/YAML input."""
+    """Validates solver configuration from dict/YAML input.
+
+    !!! warning
+        Enabling ``cvxpygen`` currently disables sparse parameter declarations.
+        cvxpygen does not yet support the N-D sparsity indices used by
+        OpenSCvx's tiled parameters, so all parameters are created as dense
+        when code generation is active.
+    """
 
     type: Literal["PTRSolver"] = "PTRSolver"
     cvx_solver: str = "QOCO"

{openscvx-0.4.1.dev169 → openscvx-0.4.1.dev171}/openscvx/solvers/ptr_solver.py

@@ -119,6 +119,14 @@ class PTRSolver(ConvexSolver):
             ``{"abstol": 1e-6, "reltol": 1e-9, "enforce_dpp": True}``.
         cvxpygen: Enable CVXPy code generation for faster solves.
             Defaults to ``False``.
+
+            !!! warning
+                Enabling cvxpygen currently disables sparse parameter
+                declarations. cvxpygen does not yet support the N-D sparsity
+                indices used by OpenSCvx's tiled parameters, so all parameters
+                are created as dense when code generation is active. This may
+                increase the generated solver's memory footprint and compile
+                time but does not affect solution correctness.
         cvxpygen_override: Overwrite existing generated solver directory
             without prompting. Defaults to ``False``.
 
@@ -234,6 +242,14 @@ class PTRSolver(ConvexSolver):
             B_d_sp = _tile_sparsity(B_d_pat, N - 1)
             C_d_sp = _tile_sparsity(C_d_pat, N - 1)
 
+        # TODO: (griffin-norris) Remove once cvxpygen supports N-D sparsity
+        # indices. cvxpygen's handle_sparsity() assumes 2-D (rows, cols) but
+        # our tiled parameters produce 3-D indices (slices, rows, cols).
+        # Dropping sparsity here is safe — it only affects codegen performance.
+        if self.cvxpygen:
+            A_d_sp = B_d_sp = C_d_sp = None
+            constraint_sparsity = None
+
         # Create all CVXPy variables for the OCP
         self._ocp_vars = create_cvxpy_variables(
             N=N,

{openscvx-0.4.1.dev169 → openscvx-0.4.1.dev171/openscvx.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: openscvx
-Version: 0.4.1.dev169
+Version: 0.4.1.dev171
 Summary: A general Python-based successive convexification implementation which uses a JAX backend.
 Author-email: Chris Hayner and Griffin Norris <haynec@uw.edu>
 License: Apache Software License