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 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, *h_args) -> None:
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(jnp.ones(self.n), *h_args)
141
- if jnp.allclose(test_lgh, 0):
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, *h_args) -> 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, *h_args)
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, *h_args) -> Array:
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, *h_args)
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, *h_args), h_2_as_rd1])
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, *h_args
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, *h_args)
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(self, z: ArrayLike, *h_args) -> Array: # pylint: disable=invalid-name
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, *h_args)
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, *h_args
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, *h_args)
260
+ return self.P_config(z, u_des, *args, **kwargs)
266
261
 
267
- def q_qp(self, z: Array, u_des: Array, *h_args) -> 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, *h_args)
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, *h_args
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, *h_args)
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, *h_args) -> 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, *h_args)
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, *h_args
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, *h_args),
364
- self.q_qp(z, u_des, *h_args),
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, *h_args),
368
- self.h_qp(z, u_des, *h_args),
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, *h_args) -> None:
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
- Args:
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, *h_args)
173
- if jnp.allclose(test_lgh, 0):
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 jnp.allclose(test_lgv, 0):
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, *h_args) -> 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, *h_args)
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, *h_args) -> Array:
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, *h_args)
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, *h_args), h_2_as_rd1])
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, *h_args
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, *h_args)
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(self, z: ArrayLike, *h_args) -> Array: # pylint: disable=invalid-name
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, *h_args)
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(self, z: ArrayLike, z_des: ArrayLike) -> Tuple[Array, Array]:
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, *h_args
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, *h_args) -> 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, *h_args
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
- [self.LgV(z, z_des), -1.0 * jnp.ones((self.num_clf, 1))],
405
- [-self.Lgh(z, *h_args), jnp.zeros((self.num_cbf, 1))],
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, *h_args) -> 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, *h_args)
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, *h_args
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, *h_args),
484
- self.q_qp(z, z_des, *h_args),
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, *h_args),
488
- self.h_qp(z, z_des, *h_args),
483
+ self.G_qp(z, z_des, *args, **kwargs),
484
+ self.h_qp(z, z_des, *args, **kwargs),
489
485
  )
@@ -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 barrier function relies on additional arguments other than just the state,
79
- include an initial seed for these arguments here. This is to help test the output of the barrier function.
80
- Defaults to ().
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 not isinstance(init_args, tuple):
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 = jnp.ones(self.n)
168
- u_test = jnp.ones(self.m)
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 = jnp.concatenate([h1_test, h2_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(self.alpha, self.num_cbf)
211
- self._check_class_kappa(self.alpha_2, self.num_rd2_cbf)
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, *h_args) -> Array:
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, *h_args) -> Array:
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, *h_args) -> 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, *h_args) -> 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 jnp.allclose(func(jnp.zeros(dim)), 0.0)
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 = jnp.repeat(
415
- jnp.linspace(-1e6, 1e6, n_test).reshape(n_test, 1), dim, axis=1
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(func, in_axes=0)(test_points)
418
- assert jnp.all(a[:-1, :] < a[1:, :])
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"
@@ -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 barrier function relies on additional arguments other than just the state,
79
- include an initial seed for these arguments here. This is to help test the output of the barrier function.
80
- Defaults to ().
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 = jnp.ones(self.n)
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 = jnp.concatenate([v1_test, v2_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(self.gamma, self.num_clf)
154
- self._check_class_kappa(self.gamma_2, self.num_rd2_clf)
155
- H_test = self.H(z_test)
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
@@ -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
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
- @article{morton2025oscbf,
56
- author = {Morton, Daniel and Pavone, Marco},
57
- title = {Safe, Task-Consistent Manipulation with Operational Space Control Barrier Functions},
58
- year = {2025},
59
- journal = {arXiv preprint arXiv:2503.06736},
60
- note = {Accepted to IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hangzhou, 2025},
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=lr74nIDIalkeRDRT8nDzemS9aNWxZLcL8sQaFc1mOvE,13294
4
- cbfpy/cbfs/clf_cbf.py,sha256=lhnQj8r1DeChuSo23Mf3dIO4Q_0MTtax87K8jR0Ur8Q,17311
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=7x2AjffmhSpvDTHOY04fCUIzhtA_fcHkCTVT-XyJ0Js,18976
7
- cbfpy/config/clf_cbf_config.py,sha256=b7S-pGuoqtRK1ioAmBYO5L2VNd9Uow0tL2rXWMjLae4,12699
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=qq3Zf9bNYnW7xeZjCl2btSHg0GN8xA8s-rrEWMqfUvI,3611
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=09LmusMpCgA5tuwAguZa6SpZkmEBwOxFZ3TaaMaOtnA,3773
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.3.dist-info/licenses/LICENSE,sha256=VsRPtVDiLQq5oEH67g9hLJNScJhe6URhGIkCcfLjGAA,1070
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.3.dist-info/METADATA,sha256=YyrFMUYX9u3EiW8MatkLACezDLnY3CU1es4R_VMl3jA,9328
30
- cbfpy-0.0.3.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
31
- cbfpy-0.0.3.dist-info/top_level.txt,sha256=xx7d-sX7PMBm11PwqarQZHgFO7zgCghhqCgx2LQnYXc,26
32
- cbfpy-0.0.3.dist-info/RECORD,,
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,,
@@ -1,5 +1,5 @@
1
1
  Wheel-Version: 1.0
2
- Generator: setuptools (80.9.0)
2
+ Generator: setuptools (80.10.2)
3
3
  Root-Is-Purelib: true
4
4
  Tag: py3-none-any
5
5
 
@@ -0,0 +1,2 @@
1
+ cbfpy
2
+ test
@@ -1,5 +0,0 @@
1
- cbfpy
2
- dist
3
- docs
4
- site
5
- test