cbfpy 0.0.3__py3-none-any.whl → 0.0.4__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- cbfpy/cbfs/cbf.py +35 -44
- cbfpy/cbfs/clf_cbf.py +57 -61
- cbfpy/config/cbf_config.py +51 -46
- cbfpy/config/clf_cbf_config.py +26 -19
- cbfpy/examples/drone_demo.py +4 -4
- cbfpy/examples/point_robot_obstacle_demo.py +5 -5
- {cbfpy-0.0.3.dist-info → cbfpy-0.0.4.dist-info}/METADATA +8 -7
- {cbfpy-0.0.3.dist-info → cbfpy-0.0.4.dist-info}/RECORD +11 -11
- {cbfpy-0.0.3.dist-info → cbfpy-0.0.4.dist-info}/WHEEL +1 -1
- cbfpy-0.0.4.dist-info/top_level.txt +2 -0
- cbfpy-0.0.3.dist-info/top_level.txt +0 -5
- {cbfpy-0.0.3.dist-info → cbfpy-0.0.4.dist-info}/licenses/LICENSE +0 -0
cbfpy/cbfs/cbf.py
CHANGED
|
@@ -30,6 +30,7 @@ import jax
|
|
|
30
30
|
import jax.numpy as jnp
|
|
31
31
|
from jax import Array
|
|
32
32
|
from jax.typing import ArrayLike
|
|
33
|
+
import numpy as np
|
|
33
34
|
import qpax
|
|
34
35
|
|
|
35
36
|
from cbfpy.config.cbf_config import CBFConfig
|
|
@@ -126,19 +127,16 @@ class CBF:
|
|
|
126
127
|
config.q,
|
|
127
128
|
config.solver_tol,
|
|
128
129
|
)
|
|
129
|
-
instance._validate_instance(*config.init_args)
|
|
130
|
+
instance._validate_instance(*config.init_args, **config.init_kwargs)
|
|
130
131
|
return instance
|
|
131
132
|
|
|
132
|
-
def _validate_instance(self, *
|
|
133
|
-
"""Checks that the CBF is valid; warns the user if not
|
|
133
|
+
def _validate_instance(self, *args, **kwargs) -> None:
|
|
134
|
+
"""Checks that the CBF is valid; warns the user if not"""
|
|
134
135
|
|
|
135
|
-
Args:
|
|
136
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
137
|
-
"""
|
|
138
136
|
try:
|
|
139
137
|
# TODO: Decide if this should be checked on a row-by-row basis or via the full matrix
|
|
140
|
-
test_lgh = self.Lgh(
|
|
141
|
-
if
|
|
138
|
+
test_lgh = self.Lgh(np.ones(self.n), *args, **kwargs)
|
|
139
|
+
if np.allclose(test_lgh, 0):
|
|
142
140
|
print_warning(
|
|
143
141
|
"Lgh is zero. Consider increasing the relative degree or modifying the barrier function."
|
|
144
142
|
)
|
|
@@ -149,18 +147,17 @@ class CBF:
|
|
|
149
147
|
)
|
|
150
148
|
|
|
151
149
|
@jax.jit
|
|
152
|
-
def safety_filter(self, z: Array, u_des: Array, *
|
|
150
|
+
def safety_filter(self, z: Array, u_des: Array, *args, **kwargs) -> Array:
|
|
153
151
|
"""Apply the CBF safety filter to a nominal control
|
|
154
152
|
|
|
155
153
|
Args:
|
|
156
154
|
z (Array): State, shape (n,)
|
|
157
155
|
u_des (Array): Desired control input, shape (m,)
|
|
158
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
159
156
|
|
|
160
157
|
Returns:
|
|
161
158
|
Array: Safe control input, shape (m,)
|
|
162
159
|
"""
|
|
163
|
-
P, q, A, b, G, h = self.qp_data(z, u_des, *
|
|
160
|
+
P, q, A, b, G, h = self.qp_data(z, u_des, *args, **kwargs)
|
|
164
161
|
if self.relax_qp:
|
|
165
162
|
x_qp = qpax.solve_qp_elastic_primal(
|
|
166
163
|
P,
|
|
@@ -182,12 +179,11 @@ class CBF:
|
|
|
182
179
|
)
|
|
183
180
|
return x_qp[: self.m]
|
|
184
181
|
|
|
185
|
-
def h(self, z: ArrayLike, *
|
|
182
|
+
def h(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
186
183
|
"""Barrier function(s)
|
|
187
184
|
|
|
188
185
|
Args:
|
|
189
186
|
z (ArrayLike): State, shape (n,)
|
|
190
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
191
187
|
|
|
192
188
|
Returns:
|
|
193
189
|
Array: Barrier function evaluation, shape (num_barr,)
|
|
@@ -195,16 +191,16 @@ class CBF:
|
|
|
195
191
|
|
|
196
192
|
# Take any relative-degree-2 barrier functions and convert them to relative-degree-1
|
|
197
193
|
def _h_2(state):
|
|
198
|
-
return self.h_2(state, *
|
|
194
|
+
return self.h_2(state, *args, **kwargs)
|
|
199
195
|
|
|
200
|
-
h_2, dh_2_dt = jax.jvp(_h_2, (z,), (self.f(z),))
|
|
201
|
-
h_2_as_rd1 = dh_2_dt + self.alpha_2(h_2)
|
|
196
|
+
h_2, dh_2_dt = jax.jvp(_h_2, (z,), (self.f(z, *args, **kwargs),))
|
|
197
|
+
h_2_as_rd1 = dh_2_dt + self.alpha_2(h_2, *args, **kwargs)
|
|
202
198
|
|
|
203
199
|
# Merge the relative-degree-1 and relative-degree-2 barrier functions
|
|
204
|
-
return jnp.concatenate([self.h_1(z, *
|
|
200
|
+
return jnp.concatenate([self.h_1(z, *args, **kwargs), h_2_as_rd1])
|
|
205
201
|
|
|
206
202
|
def h_and_Lfh( # pylint: disable=invalid-name
|
|
207
|
-
self, z: ArrayLike, *
|
|
203
|
+
self, z: ArrayLike, *args, **kwargs
|
|
208
204
|
) -> Tuple[Array, Array]:
|
|
209
205
|
"""Lie derivative of the barrier function(s) wrt the autonomous dynamics `f(z)`
|
|
210
206
|
|
|
@@ -212,7 +208,6 @@ class CBF:
|
|
|
212
208
|
|
|
213
209
|
Args:
|
|
214
210
|
z (ArrayLike): State, shape (n,)
|
|
215
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
216
211
|
|
|
217
212
|
Returns:
|
|
218
213
|
h (Array): Barrier function evaluation, shape (num_barr,)
|
|
@@ -222,16 +217,17 @@ class CBF:
|
|
|
222
217
|
# with the bonus benefit of also evaluating the barrier function
|
|
223
218
|
|
|
224
219
|
def _h(state):
|
|
225
|
-
return self.h(state, *
|
|
220
|
+
return self.h(state, *args, **kwargs)
|
|
226
221
|
|
|
227
|
-
return jax.jvp(_h, (z,), (self.f(z),))
|
|
222
|
+
return jax.jvp(_h, (z,), (self.f(z, *args, **kwargs),))
|
|
228
223
|
|
|
229
|
-
def Lgh(
|
|
224
|
+
def Lgh(
|
|
225
|
+
self, z: ArrayLike, *args, **kwargs
|
|
226
|
+
) -> Array: # pylint: disable=invalid-name
|
|
230
227
|
"""Lie derivative of the barrier function(s) wrt the control dynamics `g(z)u`
|
|
231
228
|
|
|
232
229
|
Args:
|
|
233
230
|
z (ArrayLike): State, shape (n,)
|
|
234
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
235
231
|
|
|
236
232
|
Returns:
|
|
237
233
|
Array: Lgh, shape (num_barr, m)
|
|
@@ -239,47 +235,45 @@ class CBF:
|
|
|
239
235
|
# Note: the below code is just a more efficient way of stating `Lgh = jax.jacobian(self.h)(z) @ self.g(z)`
|
|
240
236
|
|
|
241
237
|
def _h(state):
|
|
242
|
-
return self.h(state, *
|
|
238
|
+
return self.h(state, *args, **kwargs)
|
|
243
239
|
|
|
244
240
|
def _jvp(g_column):
|
|
245
241
|
return jax.jvp(_h, (z,), (g_column,))[1]
|
|
246
242
|
|
|
247
|
-
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z))
|
|
243
|
+
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z, *args, **kwargs))
|
|
248
244
|
|
|
249
245
|
## QP Matrices ##
|
|
250
246
|
|
|
251
247
|
def P_qp( # pylint: disable=invalid-name
|
|
252
|
-
self, z: Array, u_des: Array, *
|
|
248
|
+
self, z: Array, u_des: Array, *args, **kwargs
|
|
253
249
|
) -> Array:
|
|
254
250
|
"""Quadratic term in the QP objective (`minimize 0.5 * x^T P x + q^T x`)
|
|
255
251
|
|
|
256
252
|
Args:
|
|
257
253
|
z (Array): State, shape (n,)
|
|
258
254
|
u_des (Array): Desired control input, shape (m,)
|
|
259
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
260
255
|
|
|
261
256
|
Returns:
|
|
262
257
|
Array: P matrix, shape (m, m)
|
|
263
258
|
"""
|
|
264
259
|
# This is user-modifiable in the config, but defaults to 2 * I for the standard min-norm CBF objective
|
|
265
|
-
return self.P_config(z, u_des, *
|
|
260
|
+
return self.P_config(z, u_des, *args, **kwargs)
|
|
266
261
|
|
|
267
|
-
def q_qp(self, z: Array, u_des: Array, *
|
|
262
|
+
def q_qp(self, z: Array, u_des: Array, *args, **kwargs) -> Array:
|
|
268
263
|
"""Linear term in the QP objective (`minimize 0.5 * x^T P x + q^T x`)
|
|
269
264
|
|
|
270
265
|
Args:
|
|
271
266
|
z (Array): State, shape (n,)
|
|
272
267
|
u_des (Array): Desired control input, shape (m,)
|
|
273
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
274
268
|
|
|
275
269
|
Returns:
|
|
276
270
|
Array: q vector, shape (m,)
|
|
277
271
|
"""
|
|
278
272
|
# This is user-modifiable in the config, but defaults to -2 * u_des for the standard min-norm CBF objective
|
|
279
|
-
return self.q_config(z, u_des, *
|
|
273
|
+
return self.q_config(z, u_des, *args, **kwargs)
|
|
280
274
|
|
|
281
275
|
def G_qp( # pylint: disable=invalid-name
|
|
282
|
-
self, z: Array, u_des: Array, *
|
|
276
|
+
self, z: Array, u_des: Array, *args, **kwargs
|
|
283
277
|
) -> Array:
|
|
284
278
|
"""Inequality constraint matrix for the QP (`Gx <= h`)
|
|
285
279
|
|
|
@@ -291,18 +285,17 @@ class CBF:
|
|
|
291
285
|
Args:
|
|
292
286
|
z (Array): State, shape (n,)
|
|
293
287
|
u_des (Array): Desired control input, shape (m,)
|
|
294
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
295
288
|
|
|
296
289
|
Returns:
|
|
297
290
|
Array: G matrix, shape (num_constraints, m)
|
|
298
291
|
"""
|
|
299
|
-
G = -self.Lgh(z, *
|
|
292
|
+
G = -self.Lgh(z, *args, **kwargs)
|
|
300
293
|
if self.control_constrained:
|
|
301
294
|
return jnp.block([[G], [jnp.eye(self.m)], [-jnp.eye(self.m)]])
|
|
302
295
|
else:
|
|
303
296
|
return G
|
|
304
297
|
|
|
305
|
-
def h_qp(self, z: Array, u_des: Array, *
|
|
298
|
+
def h_qp(self, z: Array, u_des: Array, *args, **kwargs) -> Array:
|
|
306
299
|
"""Upper bound on constraints for the QP (`Gx <= h`)
|
|
307
300
|
|
|
308
301
|
Note:
|
|
@@ -313,13 +306,12 @@ class CBF:
|
|
|
313
306
|
Args:
|
|
314
307
|
z (Array): State, shape (n,)
|
|
315
308
|
u_des (Array): Desired control input, shape (m,)
|
|
316
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
317
309
|
|
|
318
310
|
Returns:
|
|
319
311
|
Array: h vector, shape (num_constraints,)
|
|
320
312
|
"""
|
|
321
|
-
hz, lfh = self.h_and_Lfh(z, *
|
|
322
|
-
h = self.alpha(hz) + lfh
|
|
313
|
+
hz, lfh = self.h_and_Lfh(z, *args, **kwargs)
|
|
314
|
+
h = self.alpha(hz, *args, **kwargs) + lfh
|
|
323
315
|
if self.control_constrained:
|
|
324
316
|
return jnp.concatenate(
|
|
325
317
|
[h, jnp.asarray(self.u_max), -jnp.asarray(self.u_min)]
|
|
@@ -328,7 +320,7 @@ class CBF:
|
|
|
328
320
|
return h
|
|
329
321
|
|
|
330
322
|
def qp_data(
|
|
331
|
-
self, z: Array, u_des: Array, *
|
|
323
|
+
self, z: Array, u_des: Array, *args, **kwargs
|
|
332
324
|
) -> Tuple[Array, Array, Array, Array, Array, Array]:
|
|
333
325
|
"""Constructs the QP matrices based on the current state and desired control
|
|
334
326
|
|
|
@@ -349,7 +341,6 @@ class CBF:
|
|
|
349
341
|
Args:
|
|
350
342
|
z (Array): State, shape (n,)
|
|
351
343
|
u_des (Array): Desired control input, shape (m,)
|
|
352
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
353
344
|
|
|
354
345
|
Returns:
|
|
355
346
|
P (Array): Quadratic term in the QP objective, shape (m, m)
|
|
@@ -360,10 +351,10 @@ class CBF:
|
|
|
360
351
|
h (Array): Upper bound on constraints, shape (num_constraints,)
|
|
361
352
|
"""
|
|
362
353
|
return (
|
|
363
|
-
self.P_qp(z, u_des, *
|
|
364
|
-
self.q_qp(z, u_des, *
|
|
354
|
+
self.P_qp(z, u_des, *args, **kwargs),
|
|
355
|
+
self.q_qp(z, u_des, *args, **kwargs),
|
|
365
356
|
jnp.zeros((0, self.m)), # Equality matrix (not used for CBF)
|
|
366
357
|
jnp.zeros(0), # Equality vector (not used for CBF)
|
|
367
|
-
self.G_qp(z, u_des, *
|
|
368
|
-
self.h_qp(z, u_des, *
|
|
358
|
+
self.G_qp(z, u_des, *args, **kwargs),
|
|
359
|
+
self.h_qp(z, u_des, *args, **kwargs),
|
|
369
360
|
)
|
cbfpy/cbfs/clf_cbf.py
CHANGED
|
@@ -43,6 +43,7 @@ import jax
|
|
|
43
43
|
import jax.numpy as jnp
|
|
44
44
|
from jax import Array
|
|
45
45
|
from jax.typing import ArrayLike
|
|
46
|
+
import numpy as np
|
|
46
47
|
import qpax
|
|
47
48
|
|
|
48
49
|
from cbfpy.config.clf_cbf_config import CLFCBFConfig
|
|
@@ -158,19 +159,16 @@ class CLFCBF:
|
|
|
158
159
|
config.F,
|
|
159
160
|
config.solver_tol,
|
|
160
161
|
)
|
|
161
|
-
instance._validate_instance(*config.init_args)
|
|
162
|
+
instance._validate_instance(*config.init_args, **config.init_kwargs)
|
|
162
163
|
return instance
|
|
163
164
|
|
|
164
|
-
def _validate_instance(self, *
|
|
165
|
-
"""Checks that the CLF-CBF is valid; warns the user if not
|
|
165
|
+
def _validate_instance(self, *args, **kwargs) -> None:
|
|
166
|
+
"""Checks that the CLF-CBF is valid; warns the user if not"""
|
|
166
167
|
|
|
167
|
-
|
|
168
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
169
|
-
"""
|
|
170
|
-
test_z = jnp.ones(self.n)
|
|
168
|
+
test_z = np.ones(self.n)
|
|
171
169
|
try:
|
|
172
|
-
test_lgh = self.Lgh(test_z, *
|
|
173
|
-
if
|
|
170
|
+
test_lgh = self.Lgh(test_z, *args, **kwargs)
|
|
171
|
+
if np.allclose(test_lgh, 0):
|
|
174
172
|
print_warning(
|
|
175
173
|
"Lgh is zero. Consider increasing the relative degree or modifying the barrier function."
|
|
176
174
|
)
|
|
@@ -179,26 +177,25 @@ class CLFCBF:
|
|
|
179
177
|
"Cannot test Lgh; missing additional arguments.\n"
|
|
180
178
|
+ "Please provide an initial seed for these args in the config's init_args input"
|
|
181
179
|
)
|
|
182
|
-
test_lgv = self.LgV(test_z, test_z)
|
|
183
|
-
if
|
|
180
|
+
test_lgv = self.LgV(test_z, test_z, *args, **kwargs)
|
|
181
|
+
if np.allclose(test_lgv, 0):
|
|
184
182
|
print_warning(
|
|
185
183
|
"LgV is zero. Consider increasing the relative degree or modifying the Lyapunov function."
|
|
186
184
|
)
|
|
187
185
|
|
|
188
186
|
@jax.jit
|
|
189
|
-
def controller(self, z: Array, z_des: Array, *
|
|
187
|
+
def controller(self, z: Array, z_des: Array, *args, **kwargs) -> Array:
|
|
190
188
|
"""Compute the CLF-CBF optimal control input, optimizing for the CLF objective while
|
|
191
189
|
satisfying the CBF safety constraint.
|
|
192
190
|
|
|
193
191
|
Args:
|
|
194
192
|
z (Array): State, shape (n,)
|
|
195
193
|
z_des (Array): Desired state, shape (n,)
|
|
196
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
197
194
|
|
|
198
195
|
Returns:
|
|
199
196
|
Array: Safe control input, shape (m,)
|
|
200
197
|
"""
|
|
201
|
-
P, q, A, b, G, h = self.qp_data(z, z_des, *
|
|
198
|
+
P, q, A, b, G, h = self.qp_data(z, z_des, *args, **kwargs)
|
|
202
199
|
if self.relax_qp:
|
|
203
200
|
x_qp = qpax.solve_qp_elastic_primal(
|
|
204
201
|
P,
|
|
@@ -220,12 +217,11 @@ class CLFCBF:
|
|
|
220
217
|
)
|
|
221
218
|
return x_qp[: self.m]
|
|
222
219
|
|
|
223
|
-
def h(self, z: ArrayLike, *
|
|
220
|
+
def h(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
224
221
|
"""Barrier function(s)
|
|
225
222
|
|
|
226
223
|
Args:
|
|
227
224
|
z (ArrayLike): State, shape (n,)
|
|
228
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
229
225
|
|
|
230
226
|
Returns:
|
|
231
227
|
Array: Barrier function evaluation, shape (num_barr,)
|
|
@@ -233,16 +229,16 @@ class CLFCBF:
|
|
|
233
229
|
|
|
234
230
|
# Take any relative-degree-2 barrier functions and convert them to relative-degree-1
|
|
235
231
|
def _h_2(state):
|
|
236
|
-
return self.h_2(state, *
|
|
232
|
+
return self.h_2(state, *args, **kwargs)
|
|
237
233
|
|
|
238
|
-
h_2, dh_2_dt = jax.jvp(_h_2, (z,), (self.f(z),))
|
|
239
|
-
h_2_as_rd1 = dh_2_dt + self.alpha_2(h_2)
|
|
234
|
+
h_2, dh_2_dt = jax.jvp(_h_2, (z,), (self.f(z, *args, **kwargs),))
|
|
235
|
+
h_2_as_rd1 = dh_2_dt + self.alpha_2(h_2, *args, **kwargs)
|
|
240
236
|
|
|
241
237
|
# Merge the relative-degree-1 and relative-degree-2 barrier functions
|
|
242
|
-
return jnp.concatenate([self.h_1(z, *
|
|
238
|
+
return jnp.concatenate([self.h_1(z, *args, **kwargs), h_2_as_rd1])
|
|
243
239
|
|
|
244
240
|
def h_and_Lfh( # pylint: disable=invalid-name
|
|
245
|
-
self, z: ArrayLike, *
|
|
241
|
+
self, z: ArrayLike, *args, **kwargs
|
|
246
242
|
) -> Tuple[Array, Array]:
|
|
247
243
|
"""Lie derivative of the barrier function(s) wrt the autonomous dynamics `f(z)`
|
|
248
244
|
|
|
@@ -250,7 +246,6 @@ class CLFCBF:
|
|
|
250
246
|
|
|
251
247
|
Args:
|
|
252
248
|
z (ArrayLike): State, shape (n,)
|
|
253
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
254
249
|
|
|
255
250
|
Returns:
|
|
256
251
|
h (Array): Barrier function evaluation, shape (num_barr,)
|
|
@@ -260,16 +255,17 @@ class CLFCBF:
|
|
|
260
255
|
# with the bonus benefit of also evaluating the barrier function
|
|
261
256
|
|
|
262
257
|
def _h(state):
|
|
263
|
-
return self.h(state, *
|
|
258
|
+
return self.h(state, *args, **kwargs)
|
|
264
259
|
|
|
265
|
-
return jax.jvp(_h, (z,), (self.f(z),))
|
|
260
|
+
return jax.jvp(_h, (z,), (self.f(z, *args, **kwargs),))
|
|
266
261
|
|
|
267
|
-
def Lgh(
|
|
262
|
+
def Lgh(
|
|
263
|
+
self, z: ArrayLike, *args, **kwargs
|
|
264
|
+
) -> Array: # pylint: disable=invalid-name
|
|
268
265
|
"""Lie derivative of the barrier function(s) wrt the control dynamics `g(z)u`
|
|
269
266
|
|
|
270
267
|
Args:
|
|
271
268
|
z (ArrayLike): State, shape (n,)
|
|
272
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
273
269
|
|
|
274
270
|
Returns:
|
|
275
271
|
Array: Lgh, shape (num_barr, m)
|
|
@@ -277,16 +273,16 @@ class CLFCBF:
|
|
|
277
273
|
# Note: the below code is just a more efficient way of stating `Lgh = jax.jacobian(self.h)(z) @ self.g(z)`
|
|
278
274
|
|
|
279
275
|
def _h(state):
|
|
280
|
-
return self.h(state, *
|
|
276
|
+
return self.h(state, *args, **kwargs)
|
|
281
277
|
|
|
282
278
|
def _jvp(g_column):
|
|
283
279
|
return jax.jvp(_h, (z,), (g_column,))[1]
|
|
284
280
|
|
|
285
|
-
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z))
|
|
281
|
+
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z, *args, **kwargs))
|
|
286
282
|
|
|
287
283
|
## CLF functions ##
|
|
288
284
|
|
|
289
|
-
def V(self, z: ArrayLike, z_des: ArrayLike) -> Array:
|
|
285
|
+
def V(self, z: ArrayLike, z_des: ArrayLike, *args, **kwargs) -> Array:
|
|
290
286
|
"""Control Lyapunov Function(s)
|
|
291
287
|
|
|
292
288
|
Args:
|
|
@@ -298,17 +294,19 @@ class CLFCBF:
|
|
|
298
294
|
"""
|
|
299
295
|
|
|
300
296
|
def _V_2(state):
|
|
301
|
-
return self.V_2(state, z_des)
|
|
297
|
+
return self.V_2(state, z_des, *args, **kwargs)
|
|
302
298
|
|
|
303
299
|
# Take any relative-degree-2 CLFs and convert them to relative-degree-1
|
|
304
300
|
# NOTE: If adding args to the CLF, create a wrapper func like with the barrier function
|
|
305
|
-
V_2, dV_2_dt = jax.jvp(_V_2, (z,), (self.f(z),))
|
|
306
|
-
V2_rd1 = dV_2_dt + self.gamma_2(V_2)
|
|
301
|
+
V_2, dV_2_dt = jax.jvp(_V_2, (z,), (self.f(z, *args, **kwargs),))
|
|
302
|
+
V2_rd1 = dV_2_dt + self.gamma_2(V_2, *args, **kwargs)
|
|
307
303
|
|
|
308
304
|
# Merge the relative-degree-1 and relative-degree-2 CLFs
|
|
309
|
-
return jnp.concatenate([self.V_1(z, z_des), V2_rd1])
|
|
305
|
+
return jnp.concatenate([self.V_1(z, z_des, *args, **kwargs), V2_rd1])
|
|
310
306
|
|
|
311
|
-
def V_and_LfV(
|
|
307
|
+
def V_and_LfV(
|
|
308
|
+
self, z: ArrayLike, z_des: ArrayLike, *args, **kwargs
|
|
309
|
+
) -> Tuple[Array, Array]:
|
|
312
310
|
"""Lie derivative of the CLF wrt the autonomous dynamics `f(z)`
|
|
313
311
|
|
|
314
312
|
The evaluation of the CLF is also returned "for free", a byproduct of the jacobian-vector-product
|
|
@@ -323,11 +321,11 @@ class CLFCBF:
|
|
|
323
321
|
"""
|
|
324
322
|
|
|
325
323
|
def _V(state):
|
|
326
|
-
return self.V(state, z_des)
|
|
324
|
+
return self.V(state, z_des, *args, **kwargs)
|
|
327
325
|
|
|
328
|
-
return jax.jvp(_V, (z,), (self.f(z),))
|
|
326
|
+
return jax.jvp(_V, (z,), (self.f(z, *args, **kwargs),))
|
|
329
327
|
|
|
330
|
-
def LgV(self, z: ArrayLike, z_des: ArrayLike) -> Array:
|
|
328
|
+
def LgV(self, z: ArrayLike, z_des: ArrayLike, *args, **kwargs) -> Array:
|
|
331
329
|
"""Lie derivative of the CLF wrt the control dynamics `g(z)u`
|
|
332
330
|
|
|
333
331
|
Args:
|
|
@@ -339,50 +337,48 @@ class CLFCBF:
|
|
|
339
337
|
"""
|
|
340
338
|
|
|
341
339
|
def _V(state):
|
|
342
|
-
return self.V(state, z_des)
|
|
340
|
+
return self.V(state, z_des, *args, **kwargs)
|
|
343
341
|
|
|
344
342
|
def _jvp(g_column):
|
|
345
343
|
return jax.jvp(_V, (z,), (g_column,))[1]
|
|
346
344
|
|
|
347
|
-
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z))
|
|
345
|
+
return jax.vmap(_jvp, in_axes=1, out_axes=1)(self.g(z, *args, **kwargs))
|
|
348
346
|
|
|
349
347
|
## QP Matrices ##
|
|
350
348
|
|
|
351
349
|
def P_qp( # pylint: disable=invalid-name
|
|
352
|
-
self, z: Array, z_des: Array, *
|
|
350
|
+
self, z: Array, z_des: Array, *args, **kwargs
|
|
353
351
|
) -> Array:
|
|
354
352
|
"""Quadratic term in the QP objective (`minimize 0.5 * x^T P x + q^T x`)
|
|
355
353
|
|
|
356
354
|
Args:
|
|
357
355
|
z (Array): State, shape (n,)
|
|
358
356
|
z_des (Array): Desired state, shape (n,)
|
|
359
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
360
357
|
|
|
361
358
|
Returns:
|
|
362
359
|
Array: P matrix, shape (m, m)
|
|
363
360
|
"""
|
|
364
361
|
return jnp.block(
|
|
365
362
|
[
|
|
366
|
-
[self.H(z), jnp.zeros((self.m, 1))],
|
|
363
|
+
[self.H(z, *args, **kwargs), jnp.zeros((self.m, 1))],
|
|
367
364
|
[jnp.zeros((1, self.m)), jnp.atleast_1d(self.clf_relaxation_penalty)],
|
|
368
365
|
]
|
|
369
366
|
)
|
|
370
367
|
|
|
371
|
-
def q_qp(self, z: Array, z_des: Array, *
|
|
368
|
+
def q_qp(self, z: Array, z_des: Array, *args, **kwargs) -> Array:
|
|
372
369
|
"""Linear term in the QP objective (`minimize 0.5 * x^T P x + q^T x`)
|
|
373
370
|
|
|
374
371
|
Args:
|
|
375
372
|
z (Array): State, shape (n,)
|
|
376
373
|
z_des (Array): Desired state, shape (n,)
|
|
377
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
378
374
|
|
|
379
375
|
Returns:
|
|
380
376
|
Array: Q vector, shape (m,)
|
|
381
377
|
"""
|
|
382
|
-
return jnp.concatenate([self.F(z), jnp.array([0.0])])
|
|
378
|
+
return jnp.concatenate([self.F(z, *args, **kwargs), jnp.array([0.0])])
|
|
383
379
|
|
|
384
380
|
def G_qp( # pylint: disable=invalid-name
|
|
385
|
-
self, z: Array, z_des: Array, *
|
|
381
|
+
self, z: Array, z_des: Array, *args, **kwargs
|
|
386
382
|
) -> Array:
|
|
387
383
|
"""Inequality constraint matrix for the QP (`Gx <= h`)
|
|
388
384
|
|
|
@@ -394,15 +390,17 @@ class CLFCBF:
|
|
|
394
390
|
Args:
|
|
395
391
|
z (Array): State, shape (n,)
|
|
396
392
|
z_des (Array): Desired state, shape (n,)
|
|
397
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
398
393
|
|
|
399
394
|
Returns:
|
|
400
395
|
Array: G matrix, shape (num_constraints, m)
|
|
401
396
|
"""
|
|
402
397
|
G = jnp.block(
|
|
403
398
|
[
|
|
404
|
-
[
|
|
405
|
-
|
|
399
|
+
[
|
|
400
|
+
self.LgV(z, z_des, *args, **kwargs),
|
|
401
|
+
-1.0 * jnp.ones((self.num_clf, 1)),
|
|
402
|
+
],
|
|
403
|
+
[-self.Lgh(z, *args, **kwargs), jnp.zeros((self.num_cbf, 1))],
|
|
406
404
|
]
|
|
407
405
|
)
|
|
408
406
|
if self.control_constrained:
|
|
@@ -416,7 +414,7 @@ class CLFCBF:
|
|
|
416
414
|
else:
|
|
417
415
|
return G
|
|
418
416
|
|
|
419
|
-
def h_qp(self, z: Array, z_des: Array, *
|
|
417
|
+
def h_qp(self, z: Array, z_des: Array, *args, **kwargs) -> Array:
|
|
420
418
|
"""Upper bound on constraints for the QP (`Gx <= h`)
|
|
421
419
|
|
|
422
420
|
Note:
|
|
@@ -427,17 +425,16 @@ class CLFCBF:
|
|
|
427
425
|
Args:
|
|
428
426
|
z (Array): State, shape (n,)
|
|
429
427
|
z_des (Array): Desired state, shape (n,)
|
|
430
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
431
428
|
|
|
432
429
|
Returns:
|
|
433
430
|
Array: h vector, shape (num_constraints,)
|
|
434
431
|
"""
|
|
435
|
-
hz, lfh = self.h_and_Lfh(z, *
|
|
436
|
-
vz, lfv = self.V_and_LfV(z, z_des)
|
|
432
|
+
hz, lfh = self.h_and_Lfh(z, *args, **kwargs)
|
|
433
|
+
vz, lfv = self.V_and_LfV(z, z_des, *args, **kwargs)
|
|
437
434
|
h = jnp.concatenate(
|
|
438
435
|
[
|
|
439
|
-
-lfv - self.gamma(vz),
|
|
440
|
-
self.alpha(hz) + lfh,
|
|
436
|
+
-lfv - self.gamma(vz, *args, **kwargs),
|
|
437
|
+
self.alpha(hz, *args, **kwargs) + lfh,
|
|
441
438
|
]
|
|
442
439
|
)
|
|
443
440
|
if self.control_constrained:
|
|
@@ -448,7 +445,7 @@ class CLFCBF:
|
|
|
448
445
|
return h
|
|
449
446
|
|
|
450
447
|
def qp_data(
|
|
451
|
-
self, z: Array, z_des: Array, *
|
|
448
|
+
self, z: Array, z_des: Array, *args, **kwargs
|
|
452
449
|
) -> Tuple[Array, Array, Array, Array, Array, Array]:
|
|
453
450
|
"""Constructs the QP matrices based on the current state and desired control
|
|
454
451
|
|
|
@@ -469,7 +466,6 @@ class CLFCBF:
|
|
|
469
466
|
Args:
|
|
470
467
|
z (Array): State, shape (n,)
|
|
471
468
|
z_des (Array): Desired state, shape (n,)
|
|
472
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
473
469
|
|
|
474
470
|
Returns:
|
|
475
471
|
P (Array): Quadratic term in the QP objective, shape (m + 1, m + 1)
|
|
@@ -480,10 +476,10 @@ class CLFCBF:
|
|
|
480
476
|
h (Array): Upper bound on constraints, shape (num_constraints,)
|
|
481
477
|
"""
|
|
482
478
|
return (
|
|
483
|
-
self.P_qp(z, z_des, *
|
|
484
|
-
self.q_qp(z, z_des, *
|
|
479
|
+
self.P_qp(z, z_des, *args, **kwargs),
|
|
480
|
+
self.q_qp(z, z_des, *args, **kwargs),
|
|
485
481
|
jnp.zeros((0, self.m + 1)), # Equality matrix (not used for CLF-CBF)
|
|
486
482
|
jnp.zeros(0), # Equality vector (not used for CLF-CBF)
|
|
487
|
-
self.G_qp(z, z_des, *
|
|
488
|
-
self.h_qp(z, z_des, *
|
|
483
|
+
self.G_qp(z, z_des, *args, **kwargs),
|
|
484
|
+
self.h_qp(z, z_des, *args, **kwargs),
|
|
489
485
|
)
|
cbfpy/config/cbf_config.py
CHANGED
|
@@ -38,7 +38,6 @@ is infeasible.
|
|
|
38
38
|
|
|
39
39
|
from typing import Optional, Callable
|
|
40
40
|
from abc import ABC, abstractmethod
|
|
41
|
-
import warnings
|
|
42
41
|
|
|
43
42
|
import numpy as np
|
|
44
43
|
import jax
|
|
@@ -75,9 +74,10 @@ class CBFConfig(ABC):
|
|
|
75
74
|
control_relaxation_penalty (float, optional): Penalty on the control constraint slack variables in the
|
|
76
75
|
relaxed QP. Defaults to 1e5. Note: only applies if relax_qp is True.
|
|
77
76
|
solver_tol (float, optional): Tolerance for the QP solver. Defaults to 1e-3.
|
|
78
|
-
init_args (tuple, optional): If your
|
|
79
|
-
include an initial seed for these
|
|
80
|
-
|
|
77
|
+
init_args (tuple, optional): If your barriers or dynamics rely on additional (non-differentiable, static shape)
|
|
78
|
+
args other than just the state, include an initial seed for these args here. Defaults to None.
|
|
79
|
+
init_kwargs (dict, optional): If your barriers or dynamics rely on additional (non-differentiable, static shape)
|
|
80
|
+
kwargs other than just the state, include an initial seed for these kwargs here. Defaults to None.
|
|
81
81
|
"""
|
|
82
82
|
|
|
83
83
|
def __init__(
|
|
@@ -90,7 +90,8 @@ class CBFConfig(ABC):
|
|
|
90
90
|
cbf_relaxation_penalty: float = 1e3,
|
|
91
91
|
control_relaxation_penalty: float = 1e5,
|
|
92
92
|
solver_tol: float = 1e-3,
|
|
93
|
-
init_args: tuple =
|
|
93
|
+
init_args: Optional[tuple] = None,
|
|
94
|
+
init_kwargs: Optional[dict] = None,
|
|
94
95
|
):
|
|
95
96
|
if not (isinstance(n, int) and n > 0):
|
|
96
97
|
raise ValueError(f"n must be a positive integer. Got: {n}")
|
|
@@ -123,10 +124,18 @@ class CBFConfig(ABC):
|
|
|
123
124
|
+ " Solution will likely be poor."
|
|
124
125
|
)
|
|
125
126
|
|
|
126
|
-
if
|
|
127
|
+
if init_args is None:
|
|
128
|
+
init_args = ()
|
|
129
|
+
elif not isinstance(init_args, tuple):
|
|
127
130
|
raise ValueError(f"init_args must be a tuple. Got: {init_args}")
|
|
128
131
|
self.init_args = init_args
|
|
129
132
|
|
|
133
|
+
if init_kwargs is None:
|
|
134
|
+
init_kwargs = {}
|
|
135
|
+
elif not isinstance(init_kwargs, dict):
|
|
136
|
+
raise ValueError(f"init_kwargs must be a dict. Got: {init_kwargs}")
|
|
137
|
+
self.init_kwargs = init_kwargs
|
|
138
|
+
|
|
130
139
|
if not (
|
|
131
140
|
isinstance(control_relaxation_penalty, (int, float))
|
|
132
141
|
and control_relaxation_penalty >= 0
|
|
@@ -164,10 +173,10 @@ class CBFConfig(ABC):
|
|
|
164
173
|
print("WARNING: Control constraints have a lower penalty than the CBFs.")
|
|
165
174
|
|
|
166
175
|
# Test if the methods are provided and verify their output dimension
|
|
167
|
-
z_test =
|
|
168
|
-
u_test =
|
|
169
|
-
f_test = self.f(z_test)
|
|
170
|
-
g_test = self.g(z_test)
|
|
176
|
+
z_test = np.ones(self.n)
|
|
177
|
+
u_test = np.ones(self.m)
|
|
178
|
+
f_test = self.f(z_test, *self.init_args, **self.init_kwargs)
|
|
179
|
+
g_test = self.g(z_test, *self.init_args, **self.init_kwargs)
|
|
171
180
|
if f_test.shape != (self.n,):
|
|
172
181
|
raise ValueError(
|
|
173
182
|
f"Invalid shape for f(z). Got {f_test.shape}, expected ({self.n},)"
|
|
@@ -177,13 +186,10 @@ class CBFConfig(ABC):
|
|
|
177
186
|
f"Invalid shape for g(z). Got {g_test.shape}, expected ({self.n}, {self.m})"
|
|
178
187
|
)
|
|
179
188
|
try:
|
|
180
|
-
h1_test = self.h_1(z_test, *self.init_args)
|
|
181
|
-
h2_test = self.h_2(z_test, *self.init_args)
|
|
189
|
+
h1_test = self.h_1(z_test, *self.init_args, **self.init_kwargs)
|
|
190
|
+
h2_test = self.h_2(z_test, *self.init_args, **self.init_kwargs)
|
|
182
191
|
except TypeError as e:
|
|
183
|
-
raise ValueError(
|
|
184
|
-
"Cannot test the barrier function; likely missing additional arguments.\n"
|
|
185
|
-
+ "Please provide an initial seed for these args in the config's init_args input"
|
|
186
|
-
) from e
|
|
192
|
+
raise ValueError("Cannot test the barrier function") from e
|
|
187
193
|
if h1_test.ndim != 1 or h2_test.ndim != 1:
|
|
188
194
|
raise ValueError("Barrier function(s) must be 1D arrays")
|
|
189
195
|
self.num_rd1_cbf = h1_test.shape[0]
|
|
@@ -194,9 +200,9 @@ class CBFConfig(ABC):
|
|
|
194
200
|
"No barrier functions provided."
|
|
195
201
|
+ "\nYou can implement this via the h_1 and/or h_2 methods in your config class"
|
|
196
202
|
)
|
|
197
|
-
h_test =
|
|
198
|
-
alpha_test = self.alpha(h_test)
|
|
199
|
-
alpha_2_test = self.alpha_2(h2_test)
|
|
203
|
+
h_test = np.concatenate([h1_test, h2_test])
|
|
204
|
+
alpha_test = self.alpha(h_test, *self.init_args, **self.init_kwargs)
|
|
205
|
+
alpha_2_test = self.alpha_2(h2_test, *self.init_args, **self.init_kwargs)
|
|
200
206
|
if alpha_test.shape != (self.num_cbf,):
|
|
201
207
|
raise ValueError(
|
|
202
208
|
f"Invalid shape for alpha(h(z)): {alpha_test.shape}. Expected ({self.num_cbf},)"
|
|
@@ -207,15 +213,16 @@ class CBFConfig(ABC):
|
|
|
207
213
|
f"Invalid shape for alpha_2(h_2(z)): {alpha_2_test.shape}. Expected ({self.num_rd2_cbf},)"
|
|
208
214
|
+ "\nCheck that the output of the alpha_2() function matches the number of RD2 CBFs"
|
|
209
215
|
)
|
|
210
|
-
self._check_class_kappa(
|
|
211
|
-
|
|
216
|
+
self._check_class_kappa(
|
|
217
|
+
self.alpha, self.num_cbf, *self.init_args, **self.init_kwargs
|
|
218
|
+
)
|
|
219
|
+
self._check_class_kappa(
|
|
220
|
+
self.alpha_2, self.num_rd2_cbf, *self.init_args, **self.init_kwargs
|
|
221
|
+
)
|
|
212
222
|
try:
|
|
213
|
-
P_test = self.P(z_test, u_test, *self.init_args)
|
|
223
|
+
P_test = self.P(z_test, u_test, *self.init_args, **self.init_kwargs)
|
|
214
224
|
except TypeError as e:
|
|
215
|
-
raise ValueError(
|
|
216
|
-
"Cannot test the P matrix; likely missing additional arguments.\n"
|
|
217
|
-
+ "Please provide an initial seed for these args in the config's init_args input"
|
|
218
|
-
) from e
|
|
225
|
+
raise ValueError("Cannot test the P matrix") from e
|
|
219
226
|
if P_test.shape != (self.m, self.m):
|
|
220
227
|
raise ValueError(
|
|
221
228
|
f"Invalid shape for P(z). Got {P_test.shape}, expected ({self.m}, {self.m})"
|
|
@@ -245,7 +252,7 @@ class CBFConfig(ABC):
|
|
|
245
252
|
## Control Affine Dynamics ##
|
|
246
253
|
|
|
247
254
|
@abstractmethod
|
|
248
|
-
def f(self, z: ArrayLike) -> Array:
|
|
255
|
+
def f(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
249
256
|
"""The uncontrolled dynamics function. Possibly nonlinear, and locally Lipschitz
|
|
250
257
|
|
|
251
258
|
i.e. the function f, such that z_dot = f(z) + g(z) u
|
|
@@ -259,7 +266,7 @@ class CBFConfig(ABC):
|
|
|
259
266
|
pass
|
|
260
267
|
|
|
261
268
|
@abstractmethod
|
|
262
|
-
def g(self, z: ArrayLike) -> Array:
|
|
269
|
+
def g(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
263
270
|
"""The control affine dynamics function. Locally Lipschitz.
|
|
264
271
|
|
|
265
272
|
i.e. the function g, such that z_dot = f(z) + g(z) u
|
|
@@ -274,7 +281,7 @@ class CBFConfig(ABC):
|
|
|
274
281
|
|
|
275
282
|
## Barriers ##
|
|
276
283
|
|
|
277
|
-
def h_1(self, z: ArrayLike, *
|
|
284
|
+
def h_1(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
278
285
|
"""Relative-degree-1 barrier function(s).
|
|
279
286
|
|
|
280
287
|
A (zeroing) CBF is a continuously-differentiable function h, such that for any state z in the interior of
|
|
@@ -289,15 +296,13 @@ class CBFConfig(ABC):
|
|
|
289
296
|
|
|
290
297
|
Args:
|
|
291
298
|
z (ArrayLike): State, shape (n,)
|
|
292
|
-
*h_args: Optional additional arguments for the barrier function. Note: If using additional args with your
|
|
293
|
-
barrier, these must be a static shape/type, or else this will trigger a recompilation in Jax.
|
|
294
299
|
|
|
295
300
|
Returns:
|
|
296
301
|
Array: Barrier function(s), shape (num_rd1_barr,)
|
|
297
302
|
"""
|
|
298
303
|
return jnp.array([])
|
|
299
304
|
|
|
300
|
-
def h_2(self, z: ArrayLike, *
|
|
305
|
+
def h_2(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
301
306
|
"""Relative-degree-2 (high-order) barrier function(s).
|
|
302
307
|
|
|
303
308
|
A (zeroing) CBF is a continuously-differentiable function h, such that for any state z in the interior of
|
|
@@ -313,8 +318,6 @@ class CBFConfig(ABC):
|
|
|
313
318
|
|
|
314
319
|
Args:
|
|
315
320
|
z (ArrayLike): State, shape (n,)
|
|
316
|
-
*h_args: Optional additional arguments for the barrier function. Note: If using additional args with your
|
|
317
|
-
barrier, these must be a static shape/type, or else this will trigger a recompilation in Jax.
|
|
318
321
|
|
|
319
322
|
Returns:
|
|
320
323
|
Array: Barrier function(s), shape (num_rd2_barr,)
|
|
@@ -323,7 +326,7 @@ class CBFConfig(ABC):
|
|
|
323
326
|
|
|
324
327
|
## Additional tuning functions ##
|
|
325
328
|
|
|
326
|
-
def alpha(self, h: ArrayLike) -> Array:
|
|
329
|
+
def alpha(self, h: ArrayLike, *args, **kwargs) -> Array:
|
|
327
330
|
"""A class Kappa function, dictating the "gain" of the barrier function(s)
|
|
328
331
|
|
|
329
332
|
For reference, a class Kappa function is a monotonically increasing function which passes through the origin.
|
|
@@ -339,7 +342,7 @@ class CBFConfig(ABC):
|
|
|
339
342
|
"""
|
|
340
343
|
return h
|
|
341
344
|
|
|
342
|
-
def alpha_2(self, h_2: ArrayLike) -> Array:
|
|
345
|
+
def alpha_2(self, h_2: ArrayLike, *args, **kwargs) -> Array:
|
|
343
346
|
"""A second class Kappa function which dictactes the "gain" associated with the relative-degree-2
|
|
344
347
|
barrier functions
|
|
345
348
|
|
|
@@ -358,7 +361,7 @@ class CBFConfig(ABC):
|
|
|
358
361
|
|
|
359
362
|
# Objective function tuning
|
|
360
363
|
|
|
361
|
-
def P(self, z: Array, u_des: Array, *
|
|
364
|
+
def P(self, z: Array, u_des: Array, *args, **kwargs) -> Array:
|
|
362
365
|
"""Quadratic term in the CBF QP objective (minimize 0.5 * x^T P x + q^T x)
|
|
363
366
|
|
|
364
367
|
This defaults to 2 * I, which is the value of P when minimizing the standard CBF objective,
|
|
@@ -369,14 +372,13 @@ class CBFConfig(ABC):
|
|
|
369
372
|
Args:
|
|
370
373
|
z (Array): State, shape (n,)
|
|
371
374
|
u_des (Array): Desired control input, shape (m,)
|
|
372
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
373
375
|
|
|
374
376
|
Returns:
|
|
375
377
|
Array: P matrix, shape (m, m)
|
|
376
378
|
"""
|
|
377
379
|
return 2 * jnp.eye(self.m)
|
|
378
380
|
|
|
379
|
-
def q(self, z: Array, u_des: Array, *
|
|
381
|
+
def q(self, z: Array, u_des: Array, *args, **kwargs) -> Array:
|
|
380
382
|
"""Linear term in the CBF QP objective (minimize 0.5 * x^T P x + q^T x)
|
|
381
383
|
|
|
382
384
|
This defaults to -2 * u_des, which is the value of q when minimizing the standard CBF objective,
|
|
@@ -387,7 +389,6 @@ class CBFConfig(ABC):
|
|
|
387
389
|
Args:
|
|
388
390
|
z (Array): State, shape (n,)
|
|
389
391
|
u_des (Array): Desired control input, shape (m,)
|
|
390
|
-
*h_args: Optional additional arguments for the barrier function.
|
|
391
392
|
|
|
392
393
|
Returns:
|
|
393
394
|
Array: q vector, shape (m,)
|
|
@@ -397,7 +398,7 @@ class CBFConfig(ABC):
|
|
|
397
398
|
## Helper functions ##
|
|
398
399
|
|
|
399
400
|
def _check_class_kappa(
|
|
400
|
-
self, func: Callable[[ArrayLike], ArrayLike], dim: int
|
|
401
|
+
self, func: Callable[[ArrayLike], ArrayLike], dim: int, *args, **kwargs
|
|
401
402
|
) -> None:
|
|
402
403
|
"""Checks that the provided function is in class Kappa
|
|
403
404
|
|
|
@@ -406,16 +407,20 @@ class CBFConfig(ABC):
|
|
|
406
407
|
dim (int): Expected dimension of the output
|
|
407
408
|
"""
|
|
408
409
|
assert isinstance(func, Callable)
|
|
410
|
+
|
|
411
|
+
def func_wrapper(x):
|
|
412
|
+
return func(x, *args, **kwargs)
|
|
413
|
+
|
|
409
414
|
try:
|
|
410
415
|
# Check that func(0) == 0
|
|
411
|
-
assert
|
|
416
|
+
assert np.allclose(func_wrapper(np.zeros(dim)), 0.0)
|
|
412
417
|
# Check that func is monotonically increasing
|
|
413
418
|
n_test = 100
|
|
414
|
-
test_points =
|
|
415
|
-
|
|
419
|
+
test_points = np.repeat(
|
|
420
|
+
np.linspace(-1e6, 1e6, n_test).reshape(n_test, 1), dim, axis=1
|
|
416
421
|
)
|
|
417
|
-
a = jax.vmap(
|
|
418
|
-
assert
|
|
422
|
+
a = jax.vmap(func_wrapper, in_axes=0)(test_points)
|
|
423
|
+
assert np.all(a[:-1, :] < a[1:, :])
|
|
419
424
|
except AssertionError as e:
|
|
420
425
|
raise ValueError(
|
|
421
426
|
f"{func.__name__} does not appear to be a class Kappa function"
|
cbfpy/config/clf_cbf_config.py
CHANGED
|
@@ -75,9 +75,10 @@ class CLFCBFConfig(CBFConfig):
|
|
|
75
75
|
control_relaxation_penalty (float, optional): Penalty on the control constraint slack variables in the
|
|
76
76
|
relaxed QP. Defaults to 1e5. Note: only applies if relax_qp is True.
|
|
77
77
|
solver_tol (float, optional): Tolerance for the QP solver. Defaults to 1e-3.
|
|
78
|
-
init_args (tuple, optional): If your
|
|
79
|
-
include an initial seed for these
|
|
80
|
-
|
|
78
|
+
init_args (tuple, optional): If your barriers or dynamics rely on additional (non-differentiable, static shape)
|
|
79
|
+
args other than just the state, include an initial seed for these args here. Defaults to None.
|
|
80
|
+
init_kwargs (dict, optional): If your barriers or dynamics rely on additional (non-differentiable, static shape)
|
|
81
|
+
kwargs other than just the state, include an initial seed for these kwargs here. Defaults to None.
|
|
81
82
|
"""
|
|
82
83
|
|
|
83
84
|
def __init__(
|
|
@@ -91,7 +92,8 @@ class CLFCBFConfig(CBFConfig):
|
|
|
91
92
|
clf_relaxation_penalty: float = 1e2,
|
|
92
93
|
control_relaxation_penalty: float = 1e5,
|
|
93
94
|
solver_tol: float = 1e-3,
|
|
94
|
-
init_args: tuple =
|
|
95
|
+
init_args: Optional[tuple] = None,
|
|
96
|
+
init_kwargs: Optional[dict] = None,
|
|
95
97
|
):
|
|
96
98
|
super().__init__(
|
|
97
99
|
n,
|
|
@@ -103,6 +105,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
103
105
|
control_relaxation_penalty,
|
|
104
106
|
solver_tol,
|
|
105
107
|
init_args,
|
|
108
|
+
init_kwargs,
|
|
106
109
|
)
|
|
107
110
|
|
|
108
111
|
if not (
|
|
@@ -124,9 +127,9 @@ class CLFCBFConfig(CBFConfig):
|
|
|
124
127
|
print("WARNING: Control constraints have a lower penalty than the CLFs")
|
|
125
128
|
|
|
126
129
|
# Check on CLF dimension
|
|
127
|
-
z_test =
|
|
128
|
-
v1_test = self.V_1(z_test, z_test)
|
|
129
|
-
v2_test = self.V_2(z_test, z_test)
|
|
130
|
+
z_test = np.ones(self.n)
|
|
131
|
+
v1_test = self.V_1(z_test, z_test, *self.init_args, **self.init_kwargs)
|
|
132
|
+
v2_test = self.V_2(z_test, z_test, *self.init_args, **self.init_kwargs)
|
|
130
133
|
if v1_test.ndim != 1 or v2_test.ndim != 1:
|
|
131
134
|
raise ValueError("CLF(s) must output 1D arrays")
|
|
132
135
|
self.num_rd1_clf = v1_test.shape[0]
|
|
@@ -137,9 +140,9 @@ class CLFCBFConfig(CBFConfig):
|
|
|
137
140
|
"No Lyanpunov functions provided."
|
|
138
141
|
+ "\nYou can implement this via the V_1 and/or V_2 methods in your config class"
|
|
139
142
|
)
|
|
140
|
-
v_test =
|
|
141
|
-
gamma_test = self.gamma(v_test)
|
|
142
|
-
gamma_2_test = self.gamma_2(v2_test)
|
|
143
|
+
v_test = np.concatenate([v1_test, v2_test])
|
|
144
|
+
gamma_test = self.gamma(v_test, *self.init_args, **self.init_kwargs)
|
|
145
|
+
gamma_2_test = self.gamma_2(v2_test, *self.init_args, **self.init_kwargs)
|
|
143
146
|
if gamma_test.shape != (self.num_clf,):
|
|
144
147
|
raise ValueError(
|
|
145
148
|
f"Invalid shape for gamma(V(z)): {gamma_test.shape}. Expected ({self.num_clf},)"
|
|
@@ -150,9 +153,13 @@ class CLFCBFConfig(CBFConfig):
|
|
|
150
153
|
f"Invalid shape for gamma_2(V_2(z)): {gamma_2_test.shape}. Expected ({self.num_rd2_clf},)"
|
|
151
154
|
+ "\nCheck that the output of the gamma_2() function matches the number of RD2 CLFs"
|
|
152
155
|
)
|
|
153
|
-
self._check_class_kappa(
|
|
154
|
-
|
|
155
|
-
|
|
156
|
+
self._check_class_kappa(
|
|
157
|
+
self.gamma, self.num_clf, *self.init_args, **self.init_kwargs
|
|
158
|
+
)
|
|
159
|
+
self._check_class_kappa(
|
|
160
|
+
self.gamma_2, self.num_rd2_clf, *self.init_args, **self.init_kwargs
|
|
161
|
+
)
|
|
162
|
+
H_test = self.H(z_test, *self.init_args, **self.init_kwargs)
|
|
156
163
|
if H_test.shape != (self.m, self.m):
|
|
157
164
|
raise ValueError(
|
|
158
165
|
f"Invalid shape for H(z): {H_test.shape}. Expected ({self.m}, {self.m})"
|
|
@@ -187,7 +194,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
187
194
|
)
|
|
188
195
|
assert len(self.constraint_relaxation_penalties) == num_qp_constraints
|
|
189
196
|
|
|
190
|
-
def V_1(self, z: ArrayLike, z_des: ArrayLike) -> Array:
|
|
197
|
+
def V_1(self, z: ArrayLike, z_des: ArrayLike, *args, **kwargs) -> Array:
|
|
191
198
|
"""Relative-Degree-1 Control Lyapunov Function (CLF)
|
|
192
199
|
|
|
193
200
|
A CLF is a positive-definite function which evaluates to zero at the equilibrium point, and is
|
|
@@ -210,7 +217,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
210
217
|
return jnp.array([])
|
|
211
218
|
|
|
212
219
|
# TODO: Check if the math behind this is actually valid
|
|
213
|
-
def V_2(self, z: ArrayLike, z_des: ArrayLike) -> Array:
|
|
220
|
+
def V_2(self, z: ArrayLike, z_des: ArrayLike, *args, **kwargs) -> Array:
|
|
214
221
|
"""Relative-Degree-2 (high-order) Control Lyapunov Function (CLF)
|
|
215
222
|
|
|
216
223
|
A CLF is a positive-definite function which evaluates to zero at the equilibrium point, and is
|
|
@@ -233,7 +240,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
233
240
|
"""
|
|
234
241
|
return jnp.array([])
|
|
235
242
|
|
|
236
|
-
def gamma(self, v: ArrayLike) -> Array:
|
|
243
|
+
def gamma(self, v: ArrayLike, *args, **kwargs) -> Array:
|
|
237
244
|
"""A class Kappa function, dictating the "gain" of the CLF
|
|
238
245
|
|
|
239
246
|
For reference, a class Kappa function is a monotonically increasing function which passes through the origin.
|
|
@@ -248,7 +255,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
248
255
|
"""
|
|
249
256
|
return v
|
|
250
257
|
|
|
251
|
-
def gamma_2(self, v_2: ArrayLike) -> Array:
|
|
258
|
+
def gamma_2(self, v_2: ArrayLike, *args, **kwargs) -> Array:
|
|
252
259
|
"""A second class Kappa function, dictating the "gain" associated with the derivative of the CLF
|
|
253
260
|
|
|
254
261
|
For reference, a class Kappa function is a monotonically increasing function which passes through the origin.
|
|
@@ -263,7 +270,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
263
270
|
"""
|
|
264
271
|
return v_2
|
|
265
272
|
|
|
266
|
-
def H(self, z: ArrayLike) -> Array:
|
|
273
|
+
def H(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
267
274
|
"""Matrix defining the quadratic control term in the CLF objective (minimize 0.5 * u^T H u + F^T u)
|
|
268
275
|
|
|
269
276
|
**Must be PSD!**
|
|
@@ -279,7 +286,7 @@ class CLFCBFConfig(CBFConfig):
|
|
|
279
286
|
"""
|
|
280
287
|
return jnp.eye(self.m)
|
|
281
288
|
|
|
282
|
-
def F(self, z: ArrayLike) -> Array:
|
|
289
|
+
def F(self, z: ArrayLike, *args, **kwargs) -> Array:
|
|
283
290
|
"""Vector defining the linear term in the CLF objective (minimize 0.5 * u^T H u + F^T u)
|
|
284
291
|
|
|
285
292
|
The default implementation is a zero vector, but this can be overridden
|
cbfpy/examples/drone_demo.py
CHANGED
|
@@ -43,15 +43,15 @@ class DroneConfig(CBFConfig):
|
|
|
43
43
|
cbf_relaxation_penalty=1e6,
|
|
44
44
|
)
|
|
45
45
|
|
|
46
|
-
def f(self, z):
|
|
46
|
+
def f(self, z, *args, **kwargs):
|
|
47
47
|
# Assume we are directly controlling the robot's velocity
|
|
48
48
|
return jnp.zeros(self.n)
|
|
49
49
|
|
|
50
|
-
def g(self, z):
|
|
50
|
+
def g(self, z, *args, **kwargs):
|
|
51
51
|
# Assume we are directly controlling the robot's velocity
|
|
52
52
|
return jnp.block([[jnp.eye(3)], [jnp.zeros((3, 3))]])
|
|
53
53
|
|
|
54
|
-
def h_1(self, z, z_obs):
|
|
54
|
+
def h_1(self, z, z_obs, **kwargs):
|
|
55
55
|
obstacle_radius = 0.25
|
|
56
56
|
robot_radius = 0.25
|
|
57
57
|
padding = 0.1
|
|
@@ -78,7 +78,7 @@ class DroneConfig(CBFConfig):
|
|
|
78
78
|
)
|
|
79
79
|
return jnp.concatenate([h_obstacle_avoidance, h_box_containment])
|
|
80
80
|
|
|
81
|
-
def alpha(self, h):
|
|
81
|
+
def alpha(self, h, *args, **kwargs):
|
|
82
82
|
return jnp.array([3.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) * h
|
|
83
83
|
|
|
84
84
|
|
|
@@ -38,17 +38,17 @@ class PointRobotObstacleConfig(CBFConfig):
|
|
|
38
38
|
init_args=(init_z_obs,),
|
|
39
39
|
)
|
|
40
40
|
|
|
41
|
-
def f(self, z):
|
|
41
|
+
def f(self, z, *args, **kwargs):
|
|
42
42
|
A = jnp.block(
|
|
43
43
|
[[jnp.zeros((3, 3)), jnp.eye(3)], [jnp.zeros((3, 3)), jnp.zeros((3, 3))]]
|
|
44
44
|
)
|
|
45
45
|
return A @ z
|
|
46
46
|
|
|
47
|
-
def g(self, z):
|
|
47
|
+
def g(self, z, *args, **kwargs):
|
|
48
48
|
B = jnp.block([[jnp.zeros((3, 3))], [jnp.eye(3) / self.mass]])
|
|
49
49
|
return B
|
|
50
50
|
|
|
51
|
-
def h_1(self, z, z_obs):
|
|
51
|
+
def h_1(self, z, z_obs, **kwargs):
|
|
52
52
|
# Distance between >= obstacle radius + robot radius + deceleration distance
|
|
53
53
|
pos_robot = z[:3]
|
|
54
54
|
vel_robot = z[3:]
|
|
@@ -69,13 +69,13 @@ class PointRobotObstacleConfig(CBFConfig):
|
|
|
69
69
|
]
|
|
70
70
|
)
|
|
71
71
|
|
|
72
|
-
def h_2(self, z, z_obs):
|
|
72
|
+
def h_2(self, z, z_obs, **kwargs):
|
|
73
73
|
# Stay inside the safe set (a box)
|
|
74
74
|
pos_max = jnp.array([1.0, 1.0, 1.0])
|
|
75
75
|
pos_min = jnp.array([-1.0, -1.0, -1.0])
|
|
76
76
|
return jnp.concatenate([pos_max - z[:3], z[:3] - pos_min])
|
|
77
77
|
|
|
78
|
-
def alpha(self, h):
|
|
78
|
+
def alpha(self, h, *args, **kwargs):
|
|
79
79
|
return 3 * h
|
|
80
80
|
|
|
81
81
|
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: cbfpy
|
|
3
|
-
Version: 0.0.
|
|
3
|
+
Version: 0.0.4
|
|
4
4
|
Summary: Control Barrier Functions in Python
|
|
5
5
|
Author-email: Daniel Morton <danielpmorton@gmail.com>
|
|
6
6
|
Project-URL: Documentation, https://danielpmorton.github.io/cbfpy/
|
|
@@ -52,12 +52,13 @@ For API reference, see the following [documentation](https://danielpmorton.githu
|
|
|
52
52
|
If you use CBFpy in your research, please cite the following [paper](https://arxiv.org/abs/2503.06736):
|
|
53
53
|
|
|
54
54
|
```
|
|
55
|
-
@
|
|
56
|
-
author
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
55
|
+
@inproceedings{morton2025oscbf,
|
|
56
|
+
author={Morton, Daniel and Pavone, Marco},
|
|
57
|
+
booktitle={2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
|
|
58
|
+
title={Safe, Task-Consistent Manipulation with Operational Space Control Barrier Functions},
|
|
59
|
+
year={2025},
|
|
60
|
+
pages={187-194},
|
|
61
|
+
doi={10.1109/IROS60139.2025.11246389}
|
|
61
62
|
}
|
|
62
63
|
```
|
|
63
64
|
|
|
@@ -1,10 +1,10 @@
|
|
|
1
1
|
cbfpy/__init__.py,sha256=Fq5snpGxff_id-Qlpl1f52OZWr9WrVaEpZhmWE9fRpI,366
|
|
2
2
|
cbfpy/cbfs/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
3
|
-
cbfpy/cbfs/cbf.py,sha256=
|
|
4
|
-
cbfpy/cbfs/clf_cbf.py,sha256=
|
|
3
|
+
cbfpy/cbfs/cbf.py,sha256=MhdnVNk37vmIiMF1_cZoV9zBUOXUHqn4L4DmPlkO9lY,12831
|
|
4
|
+
cbfpy/cbfs/clf_cbf.py,sha256=T5SWaE8kdK-i9C7PxtevytCkai3Pu8AisNa8X8rEP38,17193
|
|
5
5
|
cbfpy/config/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
6
|
-
cbfpy/config/cbf_config.py,sha256=
|
|
7
|
-
cbfpy/config/clf_cbf_config.py,sha256=
|
|
6
|
+
cbfpy/config/cbf_config.py,sha256=Q4hzvBkxmGVTJsS5E2jDkhpcUbGaC5wlddlpjHPnnHo,19076
|
|
7
|
+
cbfpy/config/clf_cbf_config.py,sha256=srMa-MlTRsvpuQTZTOPAV9nBebIFBUwqkQYE3DbdtgA,13375
|
|
8
8
|
cbfpy/envs/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
9
9
|
cbfpy/envs/arm_envs.py,sha256=mY5A3nId0CbhpwbXrXx34T_Tce0QJzj3fiaJ2YAw4K8,2785
|
|
10
10
|
cbfpy/envs/base_env.py,sha256=eaoUrGnKAb08v9Fouwu9vR5sjQYcSFO7rKHpSuL71_U,1815
|
|
@@ -13,20 +13,20 @@ cbfpy/envs/drone_env.py,sha256=tX6TLiYvweOvcWu4KIVjdJw4SPyQqwC6rsJR9NO1dp0,5761
|
|
|
13
13
|
cbfpy/envs/point_robot_envs.py,sha256=p_UK_uwUWLyDdwvsFgVRWEI1Ngw1WqgadkMb8yjuj8Q,7877
|
|
14
14
|
cbfpy/examples/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
15
15
|
cbfpy/examples/adaptive_cruise_control_demo.py,sha256=Hr64pf_opkzRh-i2Xdo8ahbeG5Wbn-cSgBb7HK3QrhU,4209
|
|
16
|
-
cbfpy/examples/drone_demo.py,sha256=
|
|
16
|
+
cbfpy/examples/drone_demo.py,sha256=7hS3Ux7LsGVpIYg7kmfoeXs_goUh_0E8VG73he5ktjI,3672
|
|
17
17
|
cbfpy/examples/joint_limits_demo.py,sha256=erDsImVJ-VOshlmauTk6x4cZWqfdG2M5Y0HY9C_V-6w,4533
|
|
18
18
|
cbfpy/examples/point_robot_demo.py,sha256=rLHI32H76Sk-w4CTvuIo8YjgBLFZi7NLZILaGEciG9Y,2868
|
|
19
|
-
cbfpy/examples/point_robot_obstacle_demo.py,sha256=
|
|
19
|
+
cbfpy/examples/point_robot_obstacle_demo.py,sha256=QpOpZ5GRgWfRgAoKfuiPPgfsz2zdVEtmp9SKiDYHoAQ,3844
|
|
20
20
|
cbfpy/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
21
21
|
cbfpy/utils/general_utils.py,sha256=TV8pk7Miysu0H3HLe9L0MrEZLMTxOIykW_h4cIrts-o,4157
|
|
22
22
|
cbfpy/utils/jax_utils.py,sha256=87zT1_6OlaSw2HCdTfC-rqP8gchtUleSH6v_QlwzkYA,596
|
|
23
23
|
cbfpy/utils/math_utils.py,sha256=69Wb6kAuAAG38_eW39QrZHNDYYv--5hBS8RJOFsJ0BE,637
|
|
24
24
|
cbfpy/utils/visualization.py,sha256=d4bdZCEl9AUjRz-D_uNpCu4bSUkz_wOBBaTaAH9D7go,3319
|
|
25
|
-
cbfpy-0.0.
|
|
25
|
+
cbfpy-0.0.4.dist-info/licenses/LICENSE,sha256=VsRPtVDiLQq5oEH67g9hLJNScJhe6URhGIkCcfLjGAA,1070
|
|
26
26
|
test/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
27
27
|
test/test_speed.py,sha256=Trurpgin3F24XJAJNEpSvPjLjeGSKe4Jdi3uTLpRO-I,7334
|
|
28
28
|
test/test_utils.py,sha256=vf9NOPLxY9EnQStzAGhctQ21DBoggbFdMOpzbDg1o8E,829
|
|
29
|
-
cbfpy-0.0.
|
|
30
|
-
cbfpy-0.0.
|
|
31
|
-
cbfpy-0.0.
|
|
32
|
-
cbfpy-0.0.
|
|
29
|
+
cbfpy-0.0.4.dist-info/METADATA,sha256=mA08rCukDXC9VaejBGczylIZR63_jE_rWfF9niudNhI,9322
|
|
30
|
+
cbfpy-0.0.4.dist-info/WHEEL,sha256=wUyA8OaulRlbfwMtmQsvNngGrxQHAvkKcvRmdizlJi0,92
|
|
31
|
+
cbfpy-0.0.4.dist-info/top_level.txt,sha256=zf3vvm0IroFA5aatt-r1KWBtjBg-FeAh_e2ut4fLsjk,11
|
|
32
|
+
cbfpy-0.0.4.dist-info/RECORD,,
|
|
File without changes
|