Examples
All examples can be run with:
python examples/example_{name}.py
Each example creates a CBF optimizer wrapping a CBF and QP solver, then runs a matplotlib animation showing the nominal input (black arrows) vs. the CBF-filtered safe input (red arrows).
Scalar CBF
Constrains a 1D state variable to stay above or below a limit. The limit and direction change over time to demonstrate dynamic reconfiguration.
examples/example_scalar_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import ScalarCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(self, limit: float, keep_upper: bool = True) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.eye(1)
self.scalar_cbf = ScalarCBF(limit, keep_upper)
def set_parameters(self, limit: float, keep_upper: bool = True) -> None:
self.scalar_cbf.set_parameters(limit, keep_upper)
def get_parameters(self) -> tuple[float, bool]:
return self.scalar_cbf.get_parameters()
def optimize(self, nominal_input: float, curr_value: float) -> tuple[str, NDArray]:
self.scalar_cbf.calc_constraints(curr_value)
G, alpha_h = self.scalar_cbf.get_constraints()
return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, [G], [alpha_h])
def main() -> None:
optimizer = CBFOptimizer(limit=0.0)
initial_value = 0.0
value_list: list[float] = [initial_value]
time_list: list[float] = [0.0]
dt = 0.1
fig, ax = plt.subplots()
def update(
frame: int,
value_list: list[float],
time_list: list[float],
) -> None:
ax.cla()
curr_time = time_list[-1]
if curr_time < 3:
limit = -1.0
nominal_input = -1.0
keep_upper = True
elif 3 <= curr_time < 6:
limit = -0.5
nominal_input = 1.0
keep_upper = False
else:
limit = -1.5
nominal_input = -1.0
keep_upper = True
optimizer.set_parameters(limit, keep_upper)
curr_value = value_list[-1]
_, optimal_input = optimizer.optimize(nominal_input, curr_value)
value_list.append(curr_value + float(optimal_input) * dt)
time_list.append(curr_time + dt)
# show limit line
xlim = [0, 10]
ax.hlines(
limit,
*xlim,
colors="green",
linestyles="dashed",
linewidth=3,
label="keep_upper" if keep_upper else "keep_lower",
)
# show value
ax.plot(time_list, value_list, linewidth=3, label="value")
# show vector
ax.quiver(time_list[-1], value_list[-1], 0, nominal_input, scale=10, color="black")
ax.quiver(time_list[-1], value_list[-1], 0, optimal_input, scale=10, color="red")
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.set_aspect("equal")
ax.set_xlabel("Time [s]")
ax.grid()
ax.set_xlim(xlim)
ax.set_ylim([-3, 3])
ax.legend(loc="upper right")
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=100,
fargs=(
value_list,
time_list,
),
interval=10,
repeat=False,
)
# ani.save("example_scalar_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
Scalar Range CBF
Constrains a 1D state variable to stay inside or outside a range [a, b].
Uses the barrier function \(h(x) = \left(\frac{b-a}{2}\right)^2 - \left(x - \frac{a+b}{2}\right)^2\).
examples/example_scalar_range_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import ScalarRangeCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(self, a: float, b: float, keep_inside: bool = True) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.eye(1)
self.scalar_range_cbf = ScalarRangeCBF(a, b, keep_inside)
def set_parameters(self, a: float, b: float, keep_inside: bool = True) -> None:
self.scalar_range_cbf.set_parameters(a, b, keep_inside)
def get_parameters(self) -> tuple[float, float, bool]:
return self.scalar_range_cbf.get_parameters()
def optimize(self, nominal_input: float, curr_value: float) -> tuple[str, NDArray]:
self.scalar_range_cbf.calc_constraints(curr_value)
G, alpha_h = self.scalar_range_cbf.get_constraints()
return self.qp_nom_solver.optimize(np.array(nominal_input), self.P, [G], [alpha_h])
def main() -> None:
optimizer = CBFOptimizer(a=0.0, b=1.0)
initial_value = 0.0
value_list: list[float] = [initial_value]
time_list: list[float] = [0.0]
dt = 0.1
fig, ax = plt.subplots()
def update(
frame: int,
value_list: list[float],
time_list: list[float],
) -> None:
ax.cla()
curr_time = time_list[-1]
if curr_time < 3:
a = -2.0
b = -1.0
nominal_input = -1.0
keep_inside = True
elif 3 <= curr_time < 6:
a = -0.5
b = 1.0
nominal_input = 1.0
keep_inside = False
else:
a = -2.0
b = -1.0
nominal_input = -1.0
keep_inside = True
optimizer.set_parameters(a, b, keep_inside)
curr_value = value_list[-1]
_, optimal_input = optimizer.optimize(nominal_input, curr_value)
value_list.append(curr_value + float(optimal_input) * dt)
time_list.append(curr_time + dt)
# show area
ax.axhspan(
a,
b,
0,
1,
color="green",
linewidth=3,
alpha=0.5,
label="keep_inside" if keep_inside else "keep_outside",
)
# show value
ax.plot(time_list, value_list, linewidth=3, label="value")
# show vector
ax.quiver(time_list[-1], value_list[-1], 0, nominal_input, scale=10, color="black")
ax.quiver(time_list[-1], value_list[-1], 0, optimal_input, scale=10, color="red")
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.set_aspect("equal")
ax.set_xlabel("Time [s]")
ax.grid()
xlim = [0, 10]
ax.set_xlim(xlim)
ax.set_ylim([-3, 3])
ax.legend(loc="upper right")
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=100,
fargs=(
value_list,
time_list,
),
interval=10,
repeat=False,
)
# ani.save("example_scalar_range_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
Circle CBF
Two agents move toward each other with a circular collision avoidance constraint.
Each agent treats the other’s position as an obstacle with keep_inside=False.
examples/example_circle_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import CircleCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.eye(2)
self.circle_cbf = CircleCBF(center, radius, keep_inside)
def set_parameters(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
self.circle_cbf.set_parameters(center, radius, keep_inside)
def get_parameters(self) -> tuple[NDArray, float, bool]:
return self.circle_cbf.get_parameters()
def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> tuple[str, NDArray]:
self.circle_cbf.calc_constraints(agent_position)
G, alpha_h = self.circle_cbf.get_constraints()
return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h])
def main() -> None:
optimizer_list = [CBFOptimizer(np.zeros(2)), CBFOptimizer(np.zeros(2))]
initial_position_array = np.array([[-2, -2.5], [2, 2]])
agent_position_list: list[NDArray] = [initial_position_array]
dt = 0.1
fig, ax = plt.subplots()
def update(
frame: int,
agent_position_list: list[NDArray],
) -> None:
ax.cla()
curr_position_array = agent_position_list[-1]
for agent_id in range(2):
# another agent position
another_agent_position = curr_position_array[~agent_id]
radius = 1.0
keep_inside = False
optimizer_list[agent_id].set_parameters(another_agent_position, 2 * radius, keep_inside)
nominal_input = np.array([-2 * agent_id + 1] * 2)
curr_position = curr_position_array[agent_id]
_, optimal_input = optimizer_list[agent_id].optimize(nominal_input, curr_position)
curr_position_array[agent_id] = curr_position_array[agent_id] + dt * optimal_input
# show area
r = patches.Circle(
xy=another_agent_position,
radius=radius,
color="green",
alpha=0.5,
)
ax.add_patch(r)
ax.plot(curr_position[0], curr_position[1], "o", linewidth=5, label="agent" + str(agent_id))
# show vector
ax.quiver(curr_position[0], curr_position[1], nominal_input[0], nominal_input[1], scale=10, color="black")
ax.quiver(curr_position[0], curr_position[1], optimal_input[0], optimal_input[1], scale=10, color="red")
agent_position_list.append(curr_position_array)
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.plot([0], [0], linewidth=5, color="green", alpha=0.5, label="keep_inside: " + str(keep_inside))
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.grid()
ax.legend(loc="upper left")
lim = [-5, 5]
ax.set_xlim(lim)
ax.set_ylim(lim)
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=100,
fargs=(agent_position_list,),
interval=10,
repeat=False,
)
# ani.save("example_circle_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
P-norm 2D CBF
An agent avoids a p-norm (ellipsoidal) shaped area. The p-norm shape generalizes circles (\(p=2\)), diamonds (\(p=1\)), and rectangles (\(p \to \infty\)).
examples/example_pnorm2d_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import Pnorm2dCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(
self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True
) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.eye(2)
self.pnorm2d_cbf = Pnorm2dCBF(center, width, theta, p, keep_inside)
def set_parameters(
self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True
) -> None:
self.pnorm2d_cbf.set_parameters(center, width, theta, p, keep_inside)
def get_parameters(self) -> tuple[NDArray, NDArray, float, float, bool]:
return self.pnorm2d_cbf.get_parameters()
def optimize(self, nominal_input: NDArray, agent_position: NDArray) -> tuple[str, NDArray]:
self.pnorm2d_cbf.calc_constraints(agent_position)
G, alpha_h = self.pnorm2d_cbf.get_constraints()
return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h])
def main() -> None:
# obstacle
center = np.array([-0.5, 0.5])
width = np.array([3, 2])
theta = -0.3
p = 2.0
keep_inside = False
optimizer = CBFOptimizer(center, width, theta, p, keep_inside)
initial_position = np.array([-3, -2.5])
agent_position_list: list[NDArray] = [initial_position]
dt = 0.1
nominal_input = np.ones(2)
fig, ax = plt.subplots()
def update(
frame: int,
agent_position_list: list[NDArray],
) -> None:
ax.cla()
curr_position = agent_position_list[-1]
_, optimal_input = optimizer.optimize(nominal_input, curr_position)
agent_position_list.append(curr_position + dt * optimal_input)
# show area
r = patches.Ellipse(
xy=center,
width=width[0] * 2,
height=width[1] * 2,
angle=theta * 180 / np.pi,
color="green",
alpha=0.5,
label="keep_inside: " + str(keep_inside),
)
ax.add_patch(r)
ax.plot(curr_position[0], curr_position[1], "o", linewidth=5, label="agent")
# show vector
ax.quiver(curr_position[0], curr_position[1], nominal_input[0], nominal_input[1], scale=10, color="black")
ax.quiver(curr_position[0], curr_position[1], optimal_input[0], optimal_input[1], scale=10, color="red")
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.grid()
ax.legend(loc="upper right")
lim = [-5, 5]
ax.set_xlim(lim)
ax.set_ylim(lim)
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=100,
fargs=(agent_position_list,),
interval=10,
repeat=False,
)
# ani.save("example_pnorm2d_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
Unicycle Circle CBF
Circular area constraint for a unicycle model (input: linear velocity + angular velocity). The unicycle kinematics are handled by an input transformation matrix.
examples/example_unicycle_circle_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import UnicycleCircleCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.eye(2)
self.circle_cbf = UnicycleCircleCBF(center, radius, keep_inside)
def set_parameters(self, center: NDArray, radius: float = 1.0, keep_inside: bool = True) -> None:
self.circle_cbf.set_parameters(center, radius, keep_inside)
def get_parameters(self) -> tuple[NDArray, float, bool]:
return self.circle_cbf.get_parameters()
def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> tuple[str, NDArray]:
self.circle_cbf.calc_constraints(agent_pose)
G, alpha_h = self.circle_cbf.get_constraints()
return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h])
def main() -> None:
center = np.array([-0.5, 0.5])
radius = 1.5
keep_inside = True
optimizer = CBFOptimizer(center, radius, keep_inside)
initial_pose_array = np.array([0, 0.5, 0.0])
agent_pose_list: list[NDArray] = [initial_pose_array]
dt = 0.1
fig, ax = plt.subplots()
def update(
frame: int,
agent_pose_list: list[NDArray],
) -> None:
ax.cla()
nominal_input = np.array([1.0, 0.3])
curr_pose = agent_pose_list[-1]
optimizer.set_parameters(center, radius, keep_inside)
_, optimal_input = optimizer.optimize(nominal_input, curr_pose)
curr_position = curr_pose[0:2]
curr_theta = curr_pose[2]
transform_matrix: NDArray = np.array(
[
[np.cos(curr_theta), 0],
[0, np.sin(curr_theta)],
[0, 1],
]
)
agent_pose_list.append(curr_pose + dt * transform_matrix @ optimal_input)
# show area
r = patches.Circle(
xy=center,
radius=radius,
color="green",
alpha=0.5,
label="keep_inside: " + str(keep_inside),
)
ax.add_patch(r)
ax.plot(curr_position[0], curr_position[1], "o", linewidth=5, label="agent")
# show vector
# v
ax.quiver(
curr_position[0],
curr_position[1],
nominal_input[0] * np.cos(curr_theta),
nominal_input[0] * np.sin(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
optimal_input[0] * np.cos(curr_theta),
optimal_input[0] * np.sin(curr_theta),
scale=10,
color="red",
)
# omega
ax.quiver(
curr_position[0],
curr_position[1],
-nominal_input[1] * np.sin(curr_theta),
nominal_input[1] * np.cos(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
-optimal_input[1] * np.sin(curr_theta),
optimal_input[1] * np.cos(curr_theta),
scale=10,
color="red",
)
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.grid()
ax.legend(loc="upper left")
lim = [-3, 3]
ax.set_xlim(lim)
ax.set_ylim(lim)
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=100,
fargs=(agent_pose_list,),
interval=10,
repeat=False,
)
# ani.save("example_unicycle_circle_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
Unicycle P-norm 2D CBF
P-norm shaped area constraint for a unicycle model. Combines the p-norm barrier function with unicycle input transformation.
examples/example_unicycle_pnorm2d_cbf.py
#!/usr/bin/env python
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import UnicyclePnorm2dCBF
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(
self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True
) -> None:
self.qp_nom_solver = CBFNomQPSolver()
self.P = np.diag([1, 10])
self.pnorm2d_cbf = UnicyclePnorm2dCBF(center, width, theta, p, keep_inside)
def set_parameters(
self, center: NDArray, width: NDArray, theta: float = 0.0, p: float = 2.0, keep_inside: bool = True
) -> None:
self.pnorm2d_cbf.set_parameters(center, width, theta, p, keep_inside)
def get_parameters(self) -> tuple[NDArray, NDArray, float, float, bool]:
return self.pnorm2d_cbf.get_parameters()
def optimize(self, nominal_input: NDArray, agent_pose: NDArray) -> tuple[str, NDArray]:
self.pnorm2d_cbf.calc_constraints(agent_pose)
G, alpha_h = self.pnorm2d_cbf.get_constraints()
return self.qp_nom_solver.optimize(nominal_input, self.P, [G], [alpha_h])
def main() -> None:
center = np.array([-0.5, 0.5])
width = np.array([3, 2])
theta = -0.3
p = 2.0
keep_inside = True
optimizer = CBFOptimizer(center, width, theta, p, keep_inside)
initial_pose_array = np.array([-1, -1, 0])
agent_pose_list: list[NDArray] = [initial_pose_array]
dt = 0.1
fig, ax = plt.subplots()
def update(
frame: int,
agent_pose_list: list[NDArray],
) -> None:
ax.cla()
nominal_input = np.array([1.0, 0.2])
curr_pose = agent_pose_list[-1]
optimizer.set_parameters(center, width, theta, p, keep_inside)
_, optimal_input = optimizer.optimize(nominal_input, curr_pose)
curr_position = curr_pose[0:2]
curr_theta = curr_pose[2]
transform_matrix: NDArray = np.array(
[
[np.cos(curr_theta), 0],
[0, np.sin(curr_theta)],
[0, 1],
]
)
agent_pose_list.append(curr_pose + dt * transform_matrix @ optimal_input)
# show area
r = patches.Ellipse(
xy=center,
width=width[0] * 2,
height=width[1] * 2,
angle=theta * 180 / np.pi,
color="green",
alpha=0.5,
label="keep_inside: " + str(keep_inside),
)
ax.add_patch(r)
ax.plot(curr_position[0], curr_position[1], "o", linewidth=5, label="agent")
# show vector
# v
ax.quiver(
curr_position[0],
curr_position[1],
nominal_input[0] * np.cos(curr_theta),
nominal_input[0] * np.sin(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
optimal_input[0] * np.cos(curr_theta),
optimal_input[0] * np.sin(curr_theta),
scale=10,
color="red",
)
# omega
ax.quiver(
curr_position[0],
curr_position[1],
-nominal_input[1] * np.sin(curr_theta),
nominal_input[1] * np.cos(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
-optimal_input[1] * np.sin(curr_theta),
optimal_input[1] * np.cos(curr_theta),
scale=10,
color="red",
)
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.grid()
ax.legend(loc="upper right")
lim = [-5, 5]
ax.set_xlim(lim)
ax.set_ylim(lim)
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=200,
fargs=(agent_pose_list,),
interval=10,
repeat=False,
)
# ani.save("example_unicycle_pnorm2d_cbf.gif")
plt.show()
if __name__ == "__main__":
main()
LiDAR CBF
LiDAR-based obstacle avoidance for a unicycle model. Simulates a 2D LiDAR sensor and creates a CBF constraint for each ray, enabling reactive navigation through environments with multiple obstacles.
examples/example_lidar_cbf.py
#!/usr/bin/env python
from abc import abstractmethod
from collections.abc import Sequence
from dataclasses import dataclass
from typing import cast
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from matplotlib.animation import FuncAnimation
from numpy.typing import NDArray
from cbfpy.cbf import LiDARCBF, rotation_matrix_2d
from cbfpy.cbf_qp_solver import CBFNomQPSolver
class CBFOptimizer:
def __init__(self, num_points: int, width: NDArray, keep_upper: bool = True) -> None:
self.qp_nom_solver = CBFNomQPSolver()
# Set weights so that angular velocity is more likely to occur.
self.P = np.diag([10, 1])
self.lidar_cbf_list = [LiDARCBF(width, keep_upper) for _ in range(num_points)]
def set_parameters(self, width: NDArray, keep_upper: bool = True) -> None:
for lidar_cbf in self.lidar_cbf_list:
lidar_cbf.set_parameters(width, keep_upper)
def get_parameters(self) -> list[tuple[NDArray, bool]]:
return [lidar_cbf.get_parameters() for lidar_cbf in self.lidar_cbf_list]
def optimize(self, nominal_input: NDArray, r: NDArray, theta: NDArray) -> tuple[str, NDArray]:
G_list: list[NDArray] = []
alpha_h_list: list[float] = []
for i, lidar_cbf in enumerate(self.lidar_cbf_list):
lidar_cbf.calc_constraints(r[i], theta[i])
G, alpha_h = lidar_cbf.get_constraints()
G_list.append(G)
alpha_h_list.append(alpha_h)
return self.qp_nom_solver.optimize(nominal_input, self.P, G_list, alpha_h_list)
class Obstacle:
@abstractmethod
def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool: ...
@abstractmethod
def plot_func(self, color: str, alpha: float) -> patches.Patch: ...
@dataclass
class CircleObstacle(Obstacle):
center: NDArray
radius: float
def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool:
return cast(
bool,
np.sqrt(
sum(
(
np.array([r * np.cos(theta + curr_pose[2]), r * np.sin(theta + curr_pose[2])])
+ np.array(curr_pose[0:2])
- self.center
)
** 2
)
)
<= self.radius,
)
def plot_func(self, color: str, alpha: float) -> patches.Patch:
return patches.Circle(xy=self.center, radius=self.radius, color=color, alpha=alpha)
@dataclass
class RectangleObstacle(Obstacle):
center: NDArray
width: NDArray
theta: float
def is_inner(self, r: float, theta: float, curr_pose: NDArray) -> bool:
# standardization to unit square
transformed_position: NDArray = (
rotation_matrix_2d(-self.theta)
@ (
np.array(
[r * np.cos(theta + curr_pose[2]), r * np.sin(theta + curr_pose[2])] + np.array(curr_pose[0:2])
)
- self.center
)
/ self.width
)
# max norm
return cast(bool, np.linalg.norm(transformed_position, ord=np.inf) <= 1)
def plot_func(self, color: str, alpha: float) -> patches.Patch:
# matplotlib>=3.6 is required
return patches.Rectangle(
xy=self.center - self.width,
width=self.width[0] * 2,
height=self.width[1] * 2,
angle=self.theta * 180 / np.pi,
rotation_point="center",
color=color,
alpha=alpha,
)
class LiDARSimulator:
def __init__(
self,
num_points: int,
range_min: float,
range_max: float,
range_step_num: int,
obstacle_list: Sequence[Obstacle],
) -> None:
self.num_points = num_points
assert 0 < range_min < range_max
self.range_min = range_min
self.range_max = range_max
self.range_step_num = range_step_num
self.obstacle_list = obstacle_list
def sim(self, curr_pose: NDArray) -> tuple[NDArray, NDArray]:
# in agent coordinate
theta_array = np.array([2 * np.pi / self.num_points * i for i in range(self.num_points)])
range_step_array = np.array(
[
(self.range_max - self.range_min) / self.range_step_num * i + self.range_min
for i in range(self.range_step_num)
]
)
def linear_search(
range_step_array: NDArray,
theta: float,
curr_pose: NDArray,
obstacle_list: Sequence[Obstacle],
) -> float:
"""Linear search for measured distance."""
for range_ in range_step_array:
if any([obstacle.is_inner(range_, theta, curr_pose) for obstacle in obstacle_list]):
return float(range_)
else:
return self.range_max
range_array = np.array(
[
linear_search(range_step_array, theta_array[i], curr_pose, self.obstacle_list)
for i in range(self.num_points)
]
)
return range_array, theta_array
def main() -> None:
num_points = 20
width = np.array([0.7, 0.5])
optimizer = CBFOptimizer(num_points, width)
initial_pose = np.array([0, -3, -0.1])
agent_pose_list: list[NDArray] = [initial_pose]
dt = 0.1
# set obstacles
obstacle_list = [
RectangleObstacle(np.array([2, -4]), np.array([2, 0.5]), 0.5),
RectangleObstacle(np.array([4, 0]), np.array([0.5, 3]), 0),
CircleObstacle(np.array([3.5, 3]), 1),
CircleObstacle(np.array([-1, 5]), 2),
CircleObstacle(np.array([0, 0]), 1.5),
RectangleObstacle(np.array([-3, -1]), np.array([0.5, 3]), 0.3),
CircleObstacle(np.array([-2, -4]), 1),
]
lidar_sim = LiDARSimulator(num_points, 0.3, 2.0, 50, obstacle_list)
fig, ax = plt.subplots()
def update(frame: int, agent_pose_list: list[NDArray]) -> None:
ax.cla()
curr_pose = agent_pose_list[-1]
curr_position = curr_pose[0:2]
curr_theta = curr_pose[2]
nominal_input = np.array([0.5, 0])
# agent coordinate
r, theta = lidar_sim.sim(curr_pose)
_, optimal_input = optimizer.optimize(nominal_input, r, theta)
transform_matrix: NDArray = np.array(
[
[np.cos(curr_theta), 0],
[np.sin(curr_theta), 0],
[0, 1],
]
)
agent_pose_list.append(curr_pose + dt * transform_matrix @ optimal_input)
# show laser rays
for i in range(lidar_sim.num_points):
if lidar_sim.range_min < r[i] < lidar_sim.range_max:
style = "b-"
elif r[i] == lidar_sim.range_max:
style = "y-"
else:
style = "g-"
ax.plot(
[
curr_position[0] + lidar_sim.range_min * np.cos(theta[i] + curr_theta),
curr_position[0] + r[i] * np.cos(theta[i] + curr_theta),
],
[
curr_position[1] + lidar_sim.range_min * np.sin(theta[i] + curr_theta),
curr_position[1] + r[i] * np.sin(theta[i] + curr_theta),
],
style,
marker=".",
markeredgewidth=0,
)
# show vector
# v
ax.quiver(
curr_position[0],
curr_position[1],
nominal_input[0] * np.cos(curr_theta),
nominal_input[0] * np.sin(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
optimal_input[0] * np.cos(curr_theta),
optimal_input[0] * np.sin(curr_theta),
scale=10,
color="red",
)
# omega
ax.quiver(
curr_position[0],
curr_position[1],
-nominal_input[1] * np.sin(curr_theta),
nominal_input[1] * np.cos(curr_theta),
scale=10,
color="black",
)
ax.quiver(
curr_position[0],
curr_position[1],
-optimal_input[1] * np.sin(curr_theta),
optimal_input[1] * np.cos(curr_theta),
scale=10,
color="red",
)
ax.plot([0], [0], linewidth=5, color="black", label="nominal_input")
ax.plot([0], [0], linewidth=5, color="red", label="optimal_input")
param_list = optimizer.get_parameters()
cbf_width = param_list[0][0]
r_patch = patches.Ellipse(
xy=curr_position,
width=cbf_width[0] * 2,
height=cbf_width[1] * 2,
angle=curr_theta * 180 / np.pi,
color="blue",
alpha=0.5,
)
ax.add_patch(r_patch)
for obstacle in obstacle_list:
ax.add_patch(obstacle.plot_func(color="green", alpha=0.5))
ax.set_aspect("equal")
ax.grid()
lim = [-5, 5]
ax.set_xlim(lim)
ax.set_ylim(lim)
ax.legend(loc="upper left")
ani = FuncAnimation( # noqa: F841
fig,
update,
frames=500,
fargs=(agent_pose_list,),
interval=10,
repeat=False,
)
# ani.save("example_lidar_cbf.gif")
plt.show()
if __name__ == "__main__":
main()