Skip to content
Snippets Groups Projects
Select Git revision
  • master default protected
1 result
Blame Permalink
display_control_affine_cbf.py 2.12 KiB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71 






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()