Trajectory Controller(s)

We want unitree to run not walk.

For now we have a P and PD controllers implemented, talk to @paul-nechifor if needed
https://github.com/dimensionalOS/dimos/blob/dev/dimos/navigation/replanning_a_star/controllers.py#L27

class Controller(Protocol):
    def advance(self, lookahead_point: NDArray[np.float64], current_odom: PoseStamped) -> Twist: ...

    def rotate(self, yaw_error: float) -> Twist: ...

    def reset_errors(self) -> None: ...

    def reset_yaw_error(self, value: float) -> None: ...

feel free to readjust type structure for something more mature or generic.

Unitree go2 can run ~3-4x faster then currently, but if we speed it up, it oscillates, overshoots etc, we likely neeed some lookahead controller that's aware of path curvature etc.

This will be relevant for ANY control we do (low level arm control, any embodyment) so we are working on a generic system

I assume input will be something like

(TargetTrajectory, MeasuredTrajectory) -> Twist

Until now we've been guessing control numbers and seeing if it "looks kinda ok or oscillates"

But we need to be more formal with this and we need to control the robot at high speeds, Hit actual limits of control we can achieve given the hardware used and latency/response rates of the control available

This means we either have a guide for user taking measurements (overshoot etc) and setting values,
(example of some form of controller tuning for 3d printers - https://github.com/Klipper3d/klipper/blob/master/docs/Resonance_Compensation.md)

or ideally - auto calibration -
(example - PID autotune for ardupilot https://ardupilot.org/copter/docs/autotune.html frigate NVR PTZ camera tracking calibration https://docs.frigate.video/configuration/autotracking/#calibration)

so we can move the robot in some 1x1m space, measure the response curves, and autotune the controllers.

Your primary goal is for unitree to work as well as possible and cover whatever we didn't think of here. For example, should we add acceptable tolerance to a trajectory requested? since precision of control doesn't matter if there is lots of free space around the robot, but should walk carefully if there isn't etc.