LQT

This example shows how to use the LQT controller to track a trajectory.

Traceback (most recent call last):
  File "/home/docs/checkouts/readthedocs.org/user_builds/rofunc/checkouts/latest/examples/planning_control/plot_example_lqt.py", line 12, in <module>
    via_points = np.load('../data/LQT_LQR/rolling_pin_1.npy')
  File "/home/docs/checkouts/readthedocs.org/user_builds/rofunc/envs/latest/lib/python3.8/site-packages/numpy/lib/npyio.py", line 417, in load
    fid = stack.enter_context(open(os_fspath(file), "rb"))
FileNotFoundError: [Errno 2] No such file or directory: '../data/LQT_LQR/rolling_pin_1.npy'

import os
import numpy as np
import rofunc as rf

via_points = np.load('../data/LQT_LQR/rolling_pin_1.npy')
filter_indices = [0, 1, 5, 10, 22, 36]
via_points = via_points[filter_indices]

cfg = rf.config.utils.get_config("./planning", "lqt")

controller = rf.planning_control.lqt.LQT(via_points, cfg)
u_hat, x_hat, mu, idx_slices = controller.solve()
rf.lqt.plot_3d_uni(x_hat, mu, idx_slices, ori=False, save=False)