Add Trajectory

Required Interface

Every trajectory class MUST implement these three methods:

class MyTrajectory:
    def eval_position(self, t: float) -> np.ndarray:
        """Return [x, y] at time t."""
        ...
    
    def eval_velocity(self, t: float) -> np.ndarray:
        """Return [ẋ, ẏ] at time t."""
        ...
    
    def eval_acceleration(self, t: float) -> np.ndarray:
        """Return [ẍ, ÿ] at time t."""
        ...

All three are needed because the CT controller uses position, velocity, AND acceleration references for feedforward.

Procedure

1. Add the class to controller/trajectory.py

Follow the pattern of existing classes. Key requirements:

• Periodic: t wraps via t_mod = t % lap_duration
• C² continuous: position, velocity, acceleration must be smooth at the wrap point
• Stay within robot workspace: typical EE range is x ∈ [0.35, 0.55], y ∈ [-0.15, 0.15] meters

2. Add a spec entry in the multi-instance script

In test_cup_manipulator_tendon_multi_instance_isaac_sim.py, add to _TRAJ_SPECS:

{"type": "my_traj", "lap": 8.0, "cx": 0.42, "cy": 0.00, "radius": 0.08},

3. Handle it in _make_trajectory()

if traj_type == "my_traj":
    return MyTrajectory(cx=spec["cx"], cy=spec["cy"], ...)

4. Export from __init__ if needed

Update controller/__init__.py if the class should be importable as from controller.trajectory import MyTrajectory.

C² Continuity Check

Verify smoothness by plotting derivatives:

t = np.linspace(0, traj.lap_duration, 1000)
pos = np.array([traj.eval_position(ti) for ti in t])
vel = np.array([traj.eval_velocity(ti) for ti in t])
acc = np.array([traj.eval_acceleration(ti) for ti in t])
# Check: vel ≈ np.gradient(pos, t, axis=0)
# Check: no jumps at t=0 / t=lap_duration