diff --git a/controls/control_affine_clf.py b/controls/control_affine_clf.py
index 2522e593391085d193b0f7248bc1eb85da27678c..948991de3d4fe118054cb014d706021ae06131e9 100644
--- a/controls/control_affine_clf.py
+++ b/controls/control_affine_clf.py
@@ -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
diff --git a/examples/display_control_affine_cbf.py b/examples/display_control_affine_cbf.py
new file mode 100644
index 0000000000000000000000000000000000000000..dc8f5232c7fd021893401a3891f49f31dc02343b
--- /dev/null
+++ b/examples/display_control_affine_cbf.py
@@ -0,0 +1,70 @@
+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()
diff --git a/vis/vis_mounted_thruster_model.py b/vis/vis_mounted_thruster_model.py
index 3cac62337e1973cd49c08f3bc4f87584bf514c59..f2026333b4f29810f2ed46c83bc48a1b58d637c9 100644
--- a/vis/vis_mounted_thruster_model.py
+++ b/vis/vis_mounted_thruster_model.py
@@ -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)