Usage#
Import the necessary packages:
import rofunc as rf
import numpy as np
from rofunc.config.utils import get_config
1. LQT variants#
1.1. LQT#
via_points = ... # [nb_via_points, via_points_dim]
u_hat, x_hat, mu, idx_slices = rf.lqt.uni(via_points)
rf.lqt.plot_3d_uni(x_hat, mu, idx_slices, ori=False, save=False)
1.2. LQT with feedback#
via_points = ... # [nb_via_points, via_points_dim]
rf.lqt.uni_fb(via_points)
1.3. LQT with control primitive#
via_points = ... # [nb_via_points, via_points_dim]
u_hat, x_hat, mu, idx_slices = rf.lqt.uni_cp(via_points)
rf.lqt.plot_3d_uni([x_hat], mu, idx_slices)
2. iLQR variants#
2.1. iLQR#
cfg = get_config('./planning', 'ilqr')
# via-points
Mu = np.array([[2, 1, -np.pi / 6], [3, 2, -np.pi / 3]]) # Via-points
Rot = np.zeros([cfg.nbPoints, 2, 2]) # Object orientation matrices
# Object rotation matrices
for t in range(cfg.nbPoints):
orn_t = Mu[t, -1]
Rot[t, :, :] = np.asarray([
[np.cos(orn_t), -np.sin(orn_t)],
[np.sin(orn_t), np.cos(orn_t)]
])
u0 = np.zeros(cfg.nbVarU * (cfg.nbData - 1)) # Initial control command
x0 = np.array([3 * np.pi / 4, -np.pi / 2, -np.pi / 4]) # Initial state
rf.lqr.uni(Mu, Rot, u0, x0, cfg)
2.2. iLQR with control primitive#
cfg = get_config('./planning', 'ilqr')
# Via-points
Mu = np.array([[2, 1, -np.pi / 2], [3, 1, -np.pi / 2]]) # Via-points
Rot = np.zeros([2, 2, cfg.nbPoints]) # Object orientation matrices
# Object rotation matrices
for t in range(cfg.nbPoints):
orn_t = Mu[t, -1]
Rot[t] = np.asarray([
[np.cos(orn_t), -np.sin(orn_t)],
[np.sin(orn_t), np.cos(orn_t)]
])
u0 = np.zeros(cfg.nbVarU * (cfg.nbData - 1)) # Initial control command
x0 = np.array([3 * np.pi / 4, -np.pi / 2, -np.pi / 4]) # Initial state
rf.lqr.uni_cp(Mu, Rot, u0, x0, cfg)