Skip to content

core

CircularObstacleCBC

Bases: NamedAffineFunc

Relative degree 1 L_f h(x) + L_g h(x) u - α h(x) > 0

Source code in bayes_cbf/car/core.py 
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
class CircularObstacleCBC(NamedAffineFunc):
    """
    Relative degree 1
    L_f h(x) + L_g h(x) u - α h(x) > 0
    """
    @store_args
    def __init__(self,
                 model,
                 center,
                 radius,
                 cbf_col_K_alpha=[2, 3],
                 name="cbf-circles",
                 dtype=torch.get_default_dtype()):
        self.model = model
        self.center = center
        self.radius = radius
        self.encoder = StateAsArray()

    def to(self, dtype):
        self.dtype = dtype
        self.model.to(dtype=dtype)

    def value(self, X):
        state, inp = self.encoder.deserialize(X)
        pos = state.pose.position[:, :2]
        distsq = ((pos - self.center)**2).sum(dim=-1)
        return distsq - self.radius**2

    def __call__ (self, x, u):
        return self.A(x) @ u - self.b(x)

    def grad_h_col(self, X_in):
        if X_in.ndim == 1:
            X = X_in.unsqueeze(0)

        with variable_required_grad(X):
            grad_h_x = torch.autograd.grad(self.value(X), X)[0]

        if X_in.ndim == 1:
            grad_h_x = grad_h_x.squeeze(0)
        return grad_h_x

    def lie_f_h_col(self, X):
        return self.grad_h_col(X).bmm( self.model.f_func(X) )

    def grad_lie_f_h_col(self, X):
        with variable_required_grad(X):
            return torch.autograd.grad(self.lie_f_h_col(X), X)[0]

    def lie2_f_h_col(self, X):
        return self.grad_lie_f_h_col(X).bmm( self.model.f_func(X) )

    def lie_g_lie_f_h_col(self, X):
        return self.grad_lie_f_h_col(X).bmm( self.model.g_func(X) )

    def lie2_fu_h_col(self, X, U):
        grad_L1h = self.grad_lie_f_h_col(X)
        return grad_L1h.bmm(self.f_func(X) + self.g_func(X).bmm(U))

    def A(self, X):
        return - self.lie_g_lie_f_h_col(X)

    def b(self, X):
        K_α = torch.tensor(self.cbf_col_K_alpha, dtype=self.dtype)
        η_b_x = torch.cat([self.value(X).unsqueeze(0),
                           self.lie_f_h_col(X).unsqueeze(0)])
        return (self.lie2_f_h_col(X) + K_α @ η_b_x)

ControlCarCBFGroundTruth

Bases: ControlCarCBFLearned

Controller that avoids learning but uses the ground truth model

Source code in bayes_cbf/car/core.py 
217
218
219
220
221
222
223
224
class ControlCarCBFGroundTruth(ControlCarCBFLearned):
    """
    Controller that avoids learning but uses the ground truth model
    """
    needs_ground_truth = False
    def __init__(self, *a, **kw):
        assert kw.pop("use_ground_truth_model", False) is False
        super().__init__(*a, use_ground_truth_model=True, **kw)

UnicycleDynamicsModel

Bases: DynamicsModel

Ẋ = f(X) + g(X) u

[ v̇ₓ ] [ 0 ] [ cos(θ), 0 ] [ v̇y ] [ 0 ] [ sin(θ), 0 ] [ ω̇ ] [ 0 ] [ 0, 1 ] [ ẋ ] = [ vₓ ] + [ 0, 0 ] [ a ] [ ẏ ] [ vy ] [ 0, 0 ] [ α ] [ θ̇ ] [ ω ] [ 0, 0 ]

Source code in bayes_cbf/car/core.py 
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
72
class UnicycleDynamicsModel(DynamicsModel):
    """
    Ẋ     =     f(X)     +   g(X)     u

    [ v̇ₓ ]   [ 0        ]   [ cos(θ), 0 ]
    [ v̇y ]   [ 0        ]   [ sin(θ), 0 ]
    [ ω̇  ]   [ 0        ]   [ 0,      1 ]
    [ ẋ  ] = [ vₓ       ] + [ 0,      0 ] [ a ]
    [ ẏ  ]   [ vy       ]   [ 0,      0 ] [ α ]
    [ θ̇  ]   [ ω        ]   [ 0,      0 ]
    """
    def __init__(self, m, n):
        self.m = 2 # [a, α]
        self.n = 6 # [vₓ, vy, ω, x, y, θ]

    @property
    def ctrl_size(self):
        return self.m

    @property
    def state_size(self):
        return self.n

    def f_func(self, X_in):
        """
                [ 0        ]
                [ 0        ]
                [ 0        ]
        f(x) =  [ v cos(θ) ]
                [ v sin(θ) ]
                [ ω        ]
        """
        X = X_in.unsqueeze(0) if X_in.dim() <= 1 else X_in
        v = X[..., 0]
        ω = X[..., 1]
        θ = X[..., 4]
        fX = X.new_zeros(X.shape)
        fX[:, 0] = v.new_zeros(v.shape)
        fX[:, 1] = ω.new_zeros(v.shape)
        fX[:, 2] = (v * θ.cos()).sum(dim=-1)
        fX[:, 3] = (v * θ.sin()).sum(dim=-1)
        fX[:, 4] = ω
        return fX.squeeze(0) if X_in.dim() <= 1 else fX

    def g_func(self, X_in):
        """
                [ cos(θ), 0 ]
                [ sin(θ), 0 ]
                [ 0,      1 ]
        g(x) =  [ 0,      0 ]
                [ 0,      0 ]
                [ 0,      0 ]
        """
        X = X_in.unsqueeze(0) if X_in.dim() <= 1 else X_in
        gX = torch.zeros((*X.shape, self.m))
        gX[..., :, :] = torch.eye(2)
        return gX.squeeze(0) if X_in.dim() <= 1 else gX

f_func(X_in) 

    [ 0        ]
    [ 0        ]
    [ 0        ]

f(x) = [ v cos(θ) ] [ v sin(θ) ] [ ω ]

Source code in bayes_cbf/car/core.py 
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
def f_func(self, X_in):
    """
            [ 0        ]
            [ 0        ]
            [ 0        ]
    f(x) =  [ v cos(θ) ]
            [ v sin(θ) ]
            [ ω        ]
    """
    X = X_in.unsqueeze(0) if X_in.dim() <= 1 else X_in
    v = X[..., 0]
    ω = X[..., 1]
    θ = X[..., 4]
    fX = X.new_zeros(X.shape)
    fX[:, 0] = v.new_zeros(v.shape)
    fX[:, 1] = ω.new_zeros(v.shape)
    fX[:, 2] = (v * θ.cos()).sum(dim=-1)
    fX[:, 3] = (v * θ.sin()).sum(dim=-1)
    fX[:, 4] = ω
    return fX.squeeze(0) if X_in.dim() <= 1 else fX

g_func(X_in) 

    [ cos(θ), 0 ]
    [ sin(θ), 0 ]
    [ 0,      1 ]

g(x) = [ 0, 0 ] [ 0, 0 ] [ 0, 0 ]

Source code in bayes_cbf/car/core.py 
60
61
62
63
64
65
66
67
68
69
70
71
72
def g_func(self, X_in):
    """
            [ cos(θ), 0 ]
            [ sin(θ), 0 ]
            [ 0,      1 ]
    g(x) =  [ 0,      0 ]
            [ 0,      0 ]
            [ 0,      0 ]
    """
    X = X_in.unsqueeze(0) if X_in.dim() <= 1 else X_in
    gX = torch.zeros((*X.shape, self.m))
    gX[..., :, :] = torch.eye(2)
    return gX.squeeze(0) if X_in.dim() <= 1 else gX

run_car_control_ground_truth()

Run save car control with ground_truth model

Source code in bayes_cbf/car/core.py 
288
289
290
291
292
293
294
295
296
297
298
299
300
301
def run_car_control_ground_truth():
    """
    Run save car control with ground_truth model
    """
    controller = ControlCarCBFLearned(mean_dynamics_model=UnicycleDynamicsModel)
    start, inp = StateAsArray().deserialize(torch.zeros(1, controller.x_dim))
    start.pose.position[:, :2] = torch.tensor([[0,2]])
    start.pose.orientation = rotz(torch.tensor([-math.pi/2]))
    return sample_generator_trajectory(
        dynamics_model=HyundaiGenesisDynamicsModel(),
        D=1000,
        controller=controller.control,
        visualizer=UnicycleVisualizer(controller.centers, controller.radii),
        x0=StateAsArray().serialize(start, inp))