CLF controller
Contents
CLF controller#
!pip install dynamaxsys
!pip install cbfax
Collecting dynamaxsys
Downloading dynamaxsys-0.0.5-py3-none-any.whl.metadata (258 bytes)
Requirement already satisfied: jax in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from dynamaxsys) (0.4.13)
Requirement already satisfied: numpy in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from dynamaxsys) (1.24.4)
Requirement already satisfied: ml-dtypes>=0.1.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->dynamaxsys) (0.2.0)
Requirement already satisfied: opt-einsum in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->dynamaxsys) (3.4.0)
Requirement already satisfied: scipy>=1.7 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->dynamaxsys) (1.10.1)
Requirement already satisfied: importlib-metadata>=4.6 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->dynamaxsys) (8.5.0)
Requirement already satisfied: zipp>=3.20 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from importlib-metadata>=4.6->jax->dynamaxsys) (3.20.2)
Downloading dynamaxsys-0.0.5-py3-none-any.whl (5.3 kB)
Installing collected packages: dynamaxsys
Successfully installed dynamaxsys-0.0.5
Collecting cbfax
Downloading cbfax-0.0.1-py3-none-any.whl.metadata (265 bytes)
Requirement already satisfied: jax in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from cbfax) (0.4.13)
Requirement already satisfied: matplotlib in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from cbfax) (3.7.5)
Requirement already satisfied: numpy in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from cbfax) (1.24.4)
Requirement already satisfied: ml-dtypes>=0.1.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->cbfax) (0.2.0)
Requirement already satisfied: opt-einsum in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->cbfax) (3.4.0)
Requirement already satisfied: scipy>=1.7 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->cbfax) (1.10.1)
Requirement already satisfied: importlib-metadata>=4.6 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from jax->cbfax) (8.5.0)
Requirement already satisfied: contourpy>=1.0.1 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (1.1.1)
Requirement already satisfied: cycler>=0.10 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (0.12.1)
Requirement already satisfied: fonttools>=4.22.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (4.57.0)
Requirement already satisfied: kiwisolver>=1.0.1 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (1.4.7)
Requirement already satisfied: packaging>=20.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (25.0)
Requirement already satisfied: pillow>=6.2.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (10.4.0)
Requirement already satisfied: pyparsing>=2.3.1 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (3.1.4)
Requirement already satisfied: python-dateutil>=2.7 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (2.9.0.post0)
Requirement already satisfied: importlib-resources>=3.2.0 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from matplotlib->cbfax) (6.4.5)
Requirement already satisfied: zipp>=3.20 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from importlib-metadata>=4.6->jax->cbfax) (3.20.2)
Requirement already satisfied: six>=1.5 in /usr/share/miniconda/envs/__setup_conda/lib/python3.8/site-packages (from python-dateutil>=2.7->matplotlib->cbfax) (1.17.0)
Downloading cbfax-0.0.1-py3-none-any.whl (4.5 kB)
Installing collected packages: cbfax
Successfully installed cbfax-0.0.1
from dynamaxsys.unicycle import Unicycle
from dynamaxsys.base import get_discrete_time_dynamics
from cbfax.cbf import get_cbf_constraint_rd1
from cbfax.plotting import plot_halfspace
import jax
import jax.numpy as jnp
from ipywidgets import interact
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import functools
import numpy as np
def rotate_vector_ccw(vector, theta):
return jnp.array([[jnp.cos(theta), -jnp.sin(theta)],
[jnp.sin(theta), jnp.cos(theta)]]) @ vector
def plot_car(ax, state, car_length=0.5, car_width=0.3, color='lightskyblue', alpha=0.6):
pos = state[:2]
theta = state[2]
left_corner = pos + rotate_vector_ccw(0.5 * jnp.array([-car_length, -car_width]), theta)
car = Rectangle(left_corner, car_length, car_width, angle=theta * 180 / jnp.pi, color=color, alpha=alpha)
ax.add_patch(car)
v_vector = jnp.stack([pos, pos + 3 * car_length / 4 * jnp.array([np.cos(theta), np.sin(theta)])])
ax.plot(v_vector[:,0], v_vector[:,1])
return ax
dynamics = Unicycle()
Defining CLF#
\(V_1(x, y, \theta) = (x - x_g)^2 + (y - y_g)^2\)
\(V_2(x, y, \theta) = \left[\theta - \arctan\left( \frac{y_g - y}{x_g - x}\right)\right]^2\)
goal_state = jnp.array([2.5, 0., 0.])
def V1(state, goal_state):
x, y, theta = state
xg, yg, _ = goal_state
return (x - xg)**2 + (y - yg)**2
def V2(state, goal_state):
x, y, theta = state
xg, yg, _ = goal_state
return (theta - jnp.arctan2(yg - y, xg - x))**2
def clfs(state, goal_state):
return V1(state, goal_state), V2(state, goal_state)
v1 = functools.partial(V1, goal_state=goal_state)
v2 = functools.partial(V2, goal_state=goal_state)
No GPU/TPU found, falling back to CPU. (Set TF_CPP_MIN_LOG_LEVEL=0 and rerun for more info.)
# plotting set up
lim_value = 4
grid_points_N = 101
grid_points = jnp.linspace(-lim_value, lim_value, grid_points_N)
theta_points = jnp.linspace(-jnp.pi, jnp.pi, grid_points_N)
X, Y, THETA = jnp.meshgrid(grid_points, grid_points, theta_points)
batched_states = jnp.concatenate([X.reshape(-1,1), Y.reshape(-1,1), THETA.reshape(-1,1)], 1)
clf_values_grid = jax.vmap(clfs, [0, None])(batched_states, goal_state)
xmin = -3
xmax = 3
ymin = -3
ymax = 3
time = 0.
def alpha(x):
return 0.
@interact(x=(xmin, xmax, 0.1),
y=(ymin, ymax, 0.1),
theta_i=(0, grid_points_N-1))
def foo(x, y, theta_i):
theta = theta_points[theta_i]
state = jnp.array([x, y, theta])
# fig, axs = plt.subplots(1,3,figsize=(18,4))
plt.figure(figsize=(18,4))
for i in range(2):
plt.subplot(1, 3, i+1)
ax = plt.gca()
plt.contourf(X[:,:,theta_i], Y[:,:,theta_i], clf_values_grid[i].reshape(grid_points_N, grid_points_N, grid_points_N)[:,:,theta_i], 20, alpha=0.6, cmap="jet")
plt.colorbar()
plot_car(ax, state)
ax.set_title("CLF %i"%(i+1))
ax.axis("equal")
ax.set_xlim([xmin, xmax])
ax.set_ylim([ymin, ymax])
ax.grid()
ax = plt.subplot(1, 3, 3)
linear, constant = get_cbf_constraint_rd1(state, time, v1, alpha, dynamics)
plot_halfspace(linear, constant, "<=", xlim=(xmin, xmax), ylim=(ymin, ymax))
linear, constant = get_cbf_constraint_rd1(state, 0., v2, alpha, dynamics)
plot_halfspace(linear, constant, "<=", xlim=(xmin, xmax), ylim=(ymin, ymax))
ax.set_title("CLF Control Constraints")
ax.set_xlim([xmin, xmax])
ax.set_ylim([ymin, ymax])
ax.grid()
ax.set_xlabel("v")
ax.set_ylabel("$\\omega$")
ax.axis("equal")
Naive (myopic) controller#
\(z = [x, y, \theta]^T\), \(u=[v, \omega]\), \(\dot{z} = \begin{bmatrix} v\cos\theta \\ v\sin\theta \\ \omega\end{bmatrix} = \begin{bmatrix} \cos\theta & 0 \\ \sin\theta & 0 \\ 0& 1\end{bmatrix} \begin{bmatrix} v \\ \omega\end{bmatrix}\)
\[\min_{v, \omega} \; (v - v_d)^2 + \gamma\omega^2\]
\[\text{s.t.} \; \nabla V_1(z)^T f(z, u) \leq -\beta V_1(z)\]
\[ \nabla V_2(z)^T f(z, u) \leq -\beta V_2(z)\]
import cvxpy as cp
m = 2
vd = 0.5
gamma = 0.05
beta = 0.
u = cp.Variable(m)
dV1 = cp.Parameter(m)
dV2 = cp.Parameter(m)
c1 = cp.Parameter(1)
c2 = cp.Parameter(1)
ep1 = cp.Variable(1)
ep2 = cp.Variable(1)
obj = cp.Minimize( (u[0] - vd)**2 + gamma * u[1]**2 + 1000 * (ep1**2 + ep2**2))
constraints = [dV1 @ u + c1 <= ep1,
dV2 @ u + c1 <= ep2,
ep1 >= 0, ep2 >= 0,]
prob = cp.Problem(obj, constraints)
dt = 0.1
T_max = int(20 / dt)
discrete_dynamics = get_discrete_time_dynamics(dynamics, dt=dt)
state0 = jnp.array([-3., 2., -np.pi/4])
states = [state0]
controls = []
def alpha1(x):
return beta * x
def alpha2(x):
return 0.1 * x
for t in range(T_max):
state = states[t]
dV1_val, c1_val = get_cbf_constraint_rd1(state, time, v1, alpha1, dynamics)
dV2_val, c2_val = get_cbf_constraint_rd1(state, time, v2, alpha2, dynamics)
dV1.project_and_assign(dV1_val)
dV2.project_and_assign(dV2_val)
c1.project_and_assign(c1_val)
c2.project_and_assign(c2_val)
clf_value = min(v1(state), v2(state))
if clf_value < 1E-2:
print("reached goal!!")
break
prob.solve()
# states.append(discrete_dynamics(state, u.value, 0.))
states.append(state + dynamics(state, u.value) * dt)
controls.append(u.value)
states = jnp.stack(states)
controls = jnp.stack(controls)
clf_values = jax.vmap(clfs, [0, None])(states, goal_state)
reached goal!!
@interact(t=(0, states.shape[0]-1))
def foo(t):
state = states[t]
fig, axs = plt.subplots(1,3,figsize=(15,4))
theta_i = 0
ax = axs[0]
ax.plot(clf_values[0], color="blue")
ax.plot(clf_values[0][:t], color="blue", linewidth=5, label="CLF1")
ax.plot(clf_values[1], color="red")
ax.plot(clf_values[1][:t], color="red", linewidth=5, label="CLF2")
ax.grid()
ax.legend()
ax.set_title("CLF values")
ax = axs[1]
ax.contourf(X[:,:,theta_i], Y[:,:,theta_i], clf_values_grid[0].reshape(grid_points_N, grid_points_N, grid_points_N)[:,:,theta_i], 20, alpha=0.6, cmap="gist_gray")
plot_car(ax, state, alpha=1)
ax.set_title("X-Y position with CLF1 overlay")
ax.axis("equal")
ax.set_xlim([xmin, xmax])
ax.set_ylim([ymin, ymax])
ax.grid()
ax.plot(states[:t,0], states[:t,1], color="black", linestyle="--", label="trajectory")
ax.scatter(goal_state[:1], goal_state[1:2], s=100, color="red", label="goal")
ax.legend(loc="upper left")
ax = axs[2]
linear, constant = get_cbf_constraint_rd1(state, time, v1, alpha, dynamics)
plot_halfspace(linear, constant, "<=", xlim=(xmin, xmax), ylim=(ymin, ymax))
linear, constant = get_cbf_constraint_rd1(state, 0., v2, alpha, dynamics)
plot_halfspace(linear, constant, "<=", xlim=(xmin, xmax), ylim=(ymin, ymax))
ax.scatter(controls[t:t+1,0], controls[t:t+1,1], color="blue", zorder=4)
ax.axvline(vd, color="gray", linestyle="--")
ax.set_title("Control Constraints")
# ax.set_xlim([xmin, xmax])
# ax.set_ylim([ymin, ymax])
ax.grid()
ax.set_xlabel("v")
ax.set_ylabel("$\\omega$")
plt.tight_layout()