Skip to content
Snippets Groups Projects
Commit 9d262d25 authored by andersct's avatar andersct
Browse files

Extend clf controller to control barrier functions

- add cbf controller for relative degree 2 control
- add display for cbf controller
parent dcd553b9
No related branches found
No related tags found
No related merge requests found
......@@ -133,7 +133,20 @@ class MinNormRd2ClfController(object):
self.P = riccati.solve_cts_riccati_schur(
self.F, self.G, self.Q, np.eye(r)) # 2r, 2r
def __call__(self, x, t, fx=None, gx=None, dfxdx=None, **kwargs):
def __call__(self, x, t, fx=None, gx=None, dfxdx=None,
A_cbf=(), b_cbf=(), **kwargs):
"""
:param x:
:param t:
:param fx: m, |
:param gx: m, n |
:param dfxdx: m, m | jacobian of fx
:param A_cbf: k, n | for k control barrier function
constraints, if specified
:param b_cbf: k,
:param kwargs:
:return:
"""
# CLF
y = self.C.dot(x) - self.d
y_dot = self.C_dot.dot(x)
......@@ -145,11 +158,13 @@ class MinNormRd2ClfController(object):
phi_zero = LfV + self.eps_rate * V
phi_one = LgV
# input constraints
A = self.C.dot(dfxdx).dot(gx)
dLfydx = self.C.dot(dfxdx) # r, m
A = dLfydx.dot(gx) # r, n
# at minimum need r <= m,n for right inverse, so u is defined
Ainv = np.linalg.pinv(A)
Lf = self.C.dot(dfxdx).dot(fx)
AinvLf = Ainv.dot(Lf)
# - should have this inverse by relative degree 2 assumption
Ainv = np.linalg.pinv(A) # n, r
Lf = dLfydx.dot(fx) # r,
AinvLf = Ainv.dot(Lf) # n,
# solve the QP, writing constraints as Ac z <= bc
r = y.size
z0 = np.zeros((r+1,)) # z = [mu, delta]
......@@ -159,6 +174,11 @@ class MinNormRd2ClfController(object):
Ac = np.hstack((Ac, 0 * Ac[:, [0]])) # for delta
Ac[0, -1] = -1
bc = np.hstack((-phi_zero, self.u_max + AinvLf, -self.u_min - AinvLf))
if len(A_cbf) > 0:
A_cbf_mu = A_cbf.dot(Ainv) # k, r
A_cbf_mu = np.hstack((A_cbf_mu, 0 * A_cbf_mu[:, [0]]))
Ac = np.vstack((Ac, A_cbf_mu))
bc = np.hstack((bc, b_cbf + A_cbf.dot(AinvLf)))
linear_constraint = opt.LinearConstraint(Ac, bc - np.inf, bc)
res = opt.minimize(
lambda z: 0.5 * (self.c_delta * z[-1]**2 + np.linalg.norm(z[:-1])**2),
......@@ -173,6 +193,83 @@ class MinNormRd2ClfController(object):
return u_star
class MinNormRd2CbfObstacleController(object):
"""
Use a control barrier function (CBF) to enforce a minimum distance
to point obstacles.
Assumption:
The control input affects acceleration so the CBF constraint on
position is of relative degree two.
To avoid obstacles with distance d, use barrier function:
b_i(x_t) = ||p_t - p_i||^2_2 - d^2
where p_t = Cx_t \in \reals^2 is current position
and p_i \in \reals^2 is the ith obstacle's position.
The full barrier function for the k obstacles is
b(x_t) = (b_1(x_t); ...; b_k(x_t))
The CBF is enforced using linear class K functions having
the same coefficient. This controller basically makes the
constraints corresponding to the CBF, and hands these to
a CLF-based controller to use when selecting controls.
This implementation closely follows material from:
W. Xiao and C. Belta. "Control barrier functions for systems with
high relative degree," in IEEE Conference on Decision and Control
(CDC), 2019, pp. 474-479.
"""
def __init__(self, rd2clf_controller, C, c_alpha, d):
"""
:param rd2clf_controller: clf controller for systems of relative degree 2
:param C: 2, m | selects position from current state, p_t = Cx_t
:param c_alpha: coefficient for linear class K functions
:param d: minimum distance allowed to each obstacle
"""
self.rd2clf_controller = rd2clf_controller
self.C = C
self.c_alpha = c_alpha
self.d = d
self.p = np.zeros((0, 2))
def set_obstacles(self, p):
"""
:param p: k, 2 | obstacle positions
:return:
"""
self.p = p.reshape(-1, 2)
def set_setpoint(self, *args, **kwargs):
self.rd2clf_controller.set_setpoint(*args, **kwargs)
def __call__(self, x, t, fx=None, gx=None, dfxdx=None, **kwargs):
m = x.size
d_t = self.C.dot(x) - self.p # k, 2
b = np.linalg.norm(d_t, axis=1)**2 - self.d**2 # k,
dbdx = 2 * d_t.dot(self.C) # k, m
Lfb = dbdx.dot(fx) # k,
# only enforce barrier for valid initial conditions
is_phi_zero = b >= 0
is_phi_one = Lfb + self.c_alpha * b >= 0
is_valid = is_phi_zero & is_phi_one
k = is_valid.sum()
if k < 1:
return self.rd2clf_controller(x, t, fx=fx, gx=gx, dfxdx=dfxdx, **kwargs)
b = b[is_valid]
dbdx = dbdx[is_valid, :]
Lfb = Lfb[is_valid]
d2bdx2_i = 2 * self.C.T.dot(self.C) # m, m since d^2b/dx^2 is (k, m, m) tensor
dLfbdx = np.broadcast_to(d2bdx2_i.dot(fx), (k, m)) \
+ dbdx.dot(dfxdx) # k, m
Lf2b = dLfbdx.dot(fx) # k,
LgLfb = dLfbdx.dot(gx) # k, n
b_cbf = Lf2b + 2*self.c_alpha*Lfb + self.c_alpha**2 * b
A_cbf = -LgLfb
return self.rd2clf_controller(x, t, fx=fx, gx=gx, dfxdx=dfxdx,
A_cbf=A_cbf, b_cbf=b_cbf, **kwargs)
def make_C_matrix(select_inds, m):
"""
Build C matrix for common choices of y = Cx - d where
......
import numpy as np
from rigid_body_models import boat
from state_space_models import empirical_drag
from controls import stepping
from controls import control_affine_clf as ca
from vis import vis_mounted_thruster_model
from scene_elements.scene import Scene
from scene_elements.sphere import Sphere
from scene_elements.water import Water
def main_clf_rd2():
seed = np.random.randint(1000)
# seed = 539
np.random.seed(seed)
print('seed: {}'.format(seed))
DT = 0.1
boat_model = boat.DiamondMountedThrusters()
cts_sim_model = empirical_drag.EmpiricalDragCts(
boat_model,
np.array([.1, .5]) * 1.,
np.array([.5, .1]) * 1.,
)
x0 = np.array([0., 4, 1, 2, np.pi/2, np.pi/4 * 1])
x0[2:4] = np.random.rand(2) * 3
x0[5] *= np.random.randn()
x1 = np.array([0, 0, 0, 0, -np.pi/2, 0])
obstacle_xy = np.array([
[0., 2],
])
eps_rate = 1.
c_delta = 1.
c_alpha = 1.
min_distance = 1.
n = boat_model.f.shape[1]
u_min = np.ones((n,)) * -2.
u_max = np.ones((n,)) * 2.
clf_controller = ca.MinNormRd2ClfController(
eps_rate, c_delta, u_min, u_max,
)
m = x0.size
pos_selection_inds = [0, 1, 4]
vel_selection_inds = [2, 3, 5]
C = ca.make_C_matrix(pos_selection_inds, m)
cbf_controller = ca.MinNormRd2CbfObstacleController(
clf_controller, C[:2, :], c_alpha, min_distance,
)
C_dot = ca.make_C_matrix(vel_selection_inds, m)
cbf_controller.set_setpoint(C, x1[pos_selection_inds], C_dot)
cbf_controller.set_obstacles(obstacle_xy)
scene = Scene([Sphere(obstacle_xy[i], color='red')
for i in range(obstacle_xy.shape[0])] + [Water()])
T = 10
x, u = stepping.solve_cts_eqns(
cts_sim_model, x0, cbf_controller, DT, T,
cts_sim_model=cts_sim_model,
)
np.set_printoptions(suppress=True)
print(x.T.round(2))
print(u.T.round(2))
print(np.linalg.norm(x.T[:, :2] - obstacle_xy[0], axis=1) > min_distance)
vis_mounted_thruster_model.loop_sim(
boat_model, x.T[:, [0, 1]], x.T[:, 4], u.T, DT, scene=scene)
if __name__ == '__main__':
main_clf_rd2()
......@@ -17,7 +17,7 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
:param camera:
:return:
"""
fig, ax = plt.subplots(1, 2)
fig, ax = plt.subplots(1, 2, figsize=(8, 4))
k = theta.size
ax[0].set_xticks(np.mgrid[-50:50:1])
ax[0].set_yticks(np.mgrid[-50:50:1])
......@@ -26,6 +26,7 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
ax[0].set_aspect('equal')
ax[1].set_aspect('equal')
ax[0].grid(True)
ax[0].set_axisbelow(True)
artists = draw_model(ax[0], model, xy[0], theta[0])
scene.draw_top_view(ax[0])
......@@ -45,11 +46,11 @@ def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
scene.draw_image_view(ax[1], camera)
ani = animation.FuncAnimation(
fig, draw_fcn, k, fargs=(artists,), interval=50, repeat_delay=500, blit=False)
fig, draw_fcn, k, fargs=(artists,), interval=100, repeat_delay=500, blit=False)
plt.show()
def loop_sim(model, xy, theta, u, dt, **kwargs):
def loop_sim(model, xy, theta, u, dt, scene=None, **kwargs):
"""
:param model:
......@@ -57,6 +58,7 @@ def loop_sim(model, xy, theta, u, dt, **kwargs):
:param theta: k, | orientation of rigid body
:param u: k, n | control inputs
:param dt: time between sampled positions
:param scene:
:return:
"""
fig, ax = plt.subplots()
......@@ -67,8 +69,11 @@ def loop_sim(model, xy, theta, u, dt, **kwargs):
ax.set_ylim([-3, 3])
ax.set_aspect('equal')
plt.grid(True)
ax.set_axisbelow(True)
artists = draw_model(ax, model, xy[0], theta[0])
if scene:
scene.draw_top_view(ax)
time_text = ax.text(0.5, 1.05, 't = {:2.2f}s'.format(0), fontsize=14,
horizontalalignment='center',
verticalalignment='center', transform=ax.transAxes)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment