/safe_control

Safety-critical controllers for single/multi robotic navigation: CBF-QP, MPC-CBF, and etc.

Primary LanguagePython

safe_control

safe_control is a Python library that provides a unified codebase for safety-critical controllers in robotic navigation. It implements various control barrier function (CBF) based controllers and other types of safety-critical controllers.

Features

  • Implementation of various safety-critical controllers, including CBF-QP and MPC-CBF
  • Support for different robot dynamics models (e.g., unicycle, double integrator)
  • Support sensing and mapping simulation for RGB-D type camera sensors (limited FOV)
  • Support both single and multi agents navigation
  • Interactive plotting and visualization

Installation

To install safe_control, follow these steps:

  1. Clone the repository:

    git clone https://github.com/tkkim-robot/safe_control.git
cd safe_control

  2. (Optional) Create and activate a virtual environment:

  3. Install the package and its dependencie:

    python -m pip install -e .

    Or, install packages manually (see setup.py).

Getting Started

Familiarize with APIs and examples with the scripts in tracking.py

Basic Example

You can run our test example by:

python tracking.py

Alternatively, you can import LocalTrackingController from tracking.py.

from safe_control.tracking import LocalTrackingController
controller = LocalTrackingController(x_init, robot_spec,
                                control_type=control_type,
                                dt=dt,
                                show_animation=True,
                                save_animation=False,
                                ax=ax, fig=fig,
                                env=env_handler)

# assuming you have set the workspace (environment) in utils/env.py
controller.set_waypoints(waypoints)
_ _= controller.run_all_steps(tf=100)

You can also design your own navigation logic using the controller. contorl_step() function simulates one time step.

# self refers to the LocalTrackingController class.
for _ in range(int(tf / self.dt)):
    ret = self.control_step()
    self.draw_plot()
    if ret == -1:  # all waypoints reached
        break
self.export_video()

The sample results from the basic example:

Navigation with MPC-CBF controller

The green points are the given waypoints, and the blue area is the accumulated sensing footprints.

The gray circles are the obstacles that are known a priori.

Detect/Avoid Unknown Obstacles

You can also simulate online detection of unknown obstacles and avoidance of obstacles that are detected on-the-fly using safety-critical constratins. The configuration of the obstacles is (x, y, radius).

unknown_obs = np.array([[2, 2, 0.2], [3.0, 3.0, 0.3]])
controller.set_unknown_obs(unknown_obs)

# then, run simulation

The unknown obstacles are visualized in orange.

Navigation with MPC-CBF controller

Multi-Robot Example

You can simulate heterogeneous, multiple robots navigating in the same environment.

robot_spec = {
    'model': 'Unicycle2D',
    'robot_id': 0
    }
controller_0 = LocalTrackingController(x_init, robot_spec)

robot_spec = {
    'model': 'DynamicUnicycle2D',
    'robot_id': 1,
    'a_max': 1.5,
    'fov_angle': 90.0,
    'cam_range': 5.0,
    'radius': 0.25
}
controller_1 = LocalTrackingController(x_init, robot_spec)

# then, run simulation

First determine the specification of each robot (with different id), then run the simulation.

Homogeneous Robots Heterogeneous Robots

Module Breakdown

Dynamics

Supported robot dynamics can be found in the robots/ directory:

  • unicycle2D
  • dynamic_unicycle2D: A unicycle model that uses velocity as state and acceleration as input.
  • double_integrator2D
  • double_integrator2D with camera angle: under development

Positional Control Algorithms

Supported positional controllers can be found in the position_control/ directory:

  • cbf_qp: A simple CBF-QP controller for collision avoidance (ref: [1])
  • mpc_cbf: An MPC controller using discrete-time CBF (ref: [2])
  • optimal_decay_cbf_qp: A modified CBF-QP for point-wise feasibility guarantee (ref: [3])
  • optimal_decay_mpc_cbf: The same technique applied to MPC-CBF (ref: [4])

To use a specific control algorithm, specify it when initializing the LocalTrackingController:

controller = LocalTrackingController(..., control_type='cbf_qp', ...)

Attitude Control Algorithms

Supported attitude controllers can be found in the attitude_control/ directory:

  • gatekeeper: under development (ref: [5])

Customizing Environments

You can modify the environment in utils/env.py.

Visualization

The online visualization is performed using matplotlib.pyplot.ion.

It allows you to interatively scroll, resize, change view, etc. during simulation.

You can visualize and save animation by setting the arguments:

controller = LocalTrackingController(..., show_animation=True, save_animation=True)

Citing

If you find this repository useful, please consider citing our paper:

@inproceedings{kim2024learning, 
    author    = {Taekyung Kim and Robin Inho Kee and Dimitra Panagou},
    title     = {Learning to Refine Input Constrained Control Barrier Functions via Uncertainty-Aware Online Parameter Adaptation}, 
    booktitle = {{{arXiv} preprint {arXiv}:2409.14616}},
    shorttitle = {Online-Adaptive-CBF},
    year      = {2024}
}

Related Works

This repository has been utilized in several other projects. Here are some repositories that build upon or use components from this library: