Skip to content

popfishy/gcbfplus

BranchesTags

Repository files navigation

GCBF+

Jax Official Implementation of Paper: S Zhang*, Oswin So*, K Garg, C Fan: "GCBF+: A Neural Graph Control Barrier Function Framework for Distributed Safe Multi-Agent Control".

A much improved version of GCBFv0!

Dependencies

We recommend to use CONDA to install the requirements:

conda create -n gcbfplus python=3.10
conda activate gcbfplus
cd gcbfplus

Then install jax following the official instructions, and then install the rest of the dependencies:

pip install -r requirements.txt

Installation

Install GCBF:

pip install -e .

Run

Environments

We provide 3 2D environments including SingleIntegrator, DoubleIntegrator, and DubinsCar, and 2 3D environments including LinearDrone and CrazyFlie.

Algorithms

We provide algorithms including DK-GCBF(k_gcbf), InforMARL(informarl), GCBF+ (gcbf+), GCBF (gcbf), centralized CBF-QP (centralized_cbf), and decentralized CBF-QP (dec_share_cbf). Use --algo to specify the algorithm.

Hyper-parameters

To reproduce the results shown in our paper, one can refer to settings.yaml.

Train: 在所有实验中，在8个agent和8个obs环境中训练。2D：4x4 3D：2x2x2

To train the model (only GCBF+ and GCBF need training), use:

python train.py --env DubinsCar -n 8 --obs 8 --algo gcbf --area-size "(4,4)" --loss-action-coef 1e-4 --lr-actor 1e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000 --gpu 1

python train.py --algo k_gcbf+ --env CrazyFlie -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 3e-5 --lr-actor 1e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000 --use-koopman --gpu 2

python train.py --algo informarl --env DoubleIntegrator -n 8 --obs 8 --area-size "(4,4)" --steps 2000 --lr-actor 3e-4 --lr-Vl 1e-3 --ppo-batch-size 2048 --n-env-train 16

python train.py --algo informarl --env SingleIntegrator -n 8 --obs 8 --area-size "(4,4)" --steps 2000 --lr-actor 3e-4 --lr-Vl 1e-2 --ppo-batch-size 2048 --n-env-train 16

python train.py --algo informarl --env DubinsCar -n 8 --obs 8 --area-size "(4,4)" --steps 2000 --lr-actor 3e-4 --lr-Vl 5e-3 --ppo-batch-size 2048 --n-env-train 16 --gpu 2

python train.py --algo informarl --env LinearDrone -n 8 --obs 8 --area-size "(4,4)" --steps 2000 --lr-actor 3e-4 --lr-Vl 1e-3 --ppo-batch-size 2048 --n-env-train 16

python train.py --algo informarl --env CrazyFlie -n 8 --obs 8 --area-size "(4,4)" --steps 2000 --lr-actor 3e-4 --lr-Vl 1e-2 --ppo-batch-size 2048 --n-env-train 16 --gpu 2

% TODO

python train.py --algo r_gcbf+ --env DoubleIntegrator -n 8 --obs 8 --area-size "(4,4)" --loss-action-coef 1e-4 --lr-actor 1e-5 --lr-cbf 1e-5 --horizon 32 --steps 1000 --add-noise 

python train.py --algo r_gcbf+ --env LinearDrone -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 1e-4 --n-env-train 8 --lr-actor 1e-5 --lr-cbf 1e-5 --horizon 32 --steps 1000 --add-noise 

python train.py --env DubinsCar -n 8 --obs 8 --algo gcbf --area-size "(4,4)" --loss-action-coef 1e-4 --lr-actor 1e-5 --lr-cbf 1e-5 --horizon 32 --steps 1000

python train.py --algo gcbf --env LinearDrone -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 1e-4 --n-env-train 16 --lr-actor 1e-5 --lr-cbf 1e-5 --horizon 32 --gpu 2


% 效果最好
python train.py --algo k_gcbf+ --env DoubleIntegrator -n 8 --obs 8 --area-size "(4,4)" --loss-action-coef 1e-5 --lr-actor 1e-4 --lr-cbf 5e-5 --horizon 32 --steps 1000 --use-koopman  最好


python train.py --algo gcbf+ --env DubinsCar -n 8 --obs 8 --area-size "(4,4)" --loss-action-coef 1e-4 --lr-actor 5e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000

python train.py --algo k_gcbf+ --env DubinsCar -n 8 --obs 8 --area-size "(4,4)" --loss-action-coef 1e-4 --n-env-train 16 --lr-actor 5e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000 --use-koopman

python train.py --algo k_gcbf+ --env CrazyFlie -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 3e-5 --lr-actor 1e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000 --use-koopman  

python train.py --algo k_gcbf+ --env CrazyFlie -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 1e-4 --n-env-train 8 --lr-actor 5e-5 --lr-cbf 1e-3 --horizon 32 --steps 1000 --use-koopman


python train.py --algo k_gcbf+ --env DubinsCar -n 8 --obs 8 --area-size "(4,4)" --loss-action-coef 1e-4 --n-env-train 4 --lr-actor 1e-5 --lr-cbf 1e-4 --horizon 16 --steps 1000 --use-koopman 自己电脑跑（4070Ti）

python train.py --algo k_gcbf+ --env CrazyFlie -n 8 --obs 8 --area-size "(2,2)" --loss-action-coef 1e-4 --n-env-train 8 --lr-actor 5e-5 --lr-cbf 1e-4 --horizon 32 --steps 1000 --use-koopman

In our paper, we use 8 agents with 1000 training steps. The training logs will be saved in folder ./logs/<env>/<algo>/seed<seed>_<training-start-time>. We also provide the following flags:

  • -n: number of agents
  • --env: environment, including SingleIntegrator, DoubleIntegrator, DubinsCar, LinearDrone, and CrazyFlie
  • --algo: algorithm, including gcbf, gcbf+
  • --seed: random seed
  • --steps: number of training steps
  • --name: name of the experiment
  • --debug: debug mode: no recording, no saving
  • --obs: number of obstacles
  • --n-rays: number of LiDAR rays
  • --area-size: side length of the environment
  • --n-env-train: number of environments for training
  • --n-env-test: number of environments for testing
  • --log-dir: path to save the training logs
  • --eval-interval: interval of evaluation
  • --eval-epi: number of episodes for evaluation
  • --save-interval: interval of saving the model

In addition, use the following flags to specify the hyper-parameters:

  • --alpha: GCBF alpha
  • --horizon: GCBF+ look forward horizon
  • --lr-actor: learning rate of the actor
  • --lr-cbf: learning rate of the CBF
  • --loss-action-coef: coefficient of the action loss
  • --loss-h-dot-coef: coefficient of the h_dot loss
  • --loss-safe-coef: coefficient of the safe loss
  • --loss-unsafe-coef: coefficient of the unsafe loss
  • --buffer-size: size of the replay buffer

Test：

测试一：固定环境的大小L N从8->1024 SingleIntegrator和DoubleIntegrator L=8 ；DubinsCar L=16；3D环境L=4

测试二： 固定N和区域宽度L，obs从0->128 DoubleIntegrator （N=256，l=16）和（N=1024，l=32），其中障碍物是边长从[0.1，0.5]均匀采样的长方体，每个代理生成32条等距的激光雷达射线来检测障碍物 ； 3D LinearDrone （N=256，l=8）和（N=1024，l=12），其中障碍物是半径从[0.15,0.3]均匀采样的球体，每个代理生成130条等距的激光雷达射线来检测障碍物。

This should report the safety rate, goal reaching rate, and success rate of the learned model, and generate videos of the learned model in <path-to-log>/videos. Use the following flags to customize the test:

  • -n: number of agents
  • --obs: number of obstacles
  • --area-size: side length of the environment
  • --max-step: maximum number of steps for each episode, increase this if you have a large environment
  • --path: path to the log folder
  • --n-rays: number of LiDAR rays
  • --alpha: CBF alpha, used in centralized CBF-QP and decentralized CBF-QP
  • --max-travel: maximum travel distance of agents
  • --cbf: plot the CBF contour of this agent, only support 2D environments
  • --seed: random seed
  • --debug: debug mode
  • --cpu: use CPU
  • --u-ref: test the nominal controller, 代码中为LQR或者PID
  • --env: test environment (not needed if the log folder is specified)
  • --algo: test algorithm (not needed if the log folder is specified)
  • --step: test step (not needed if testing the last saved model)
  • --epi: number of episodes to test
  • --offset: offset of the random seeds
  • --no-video: do not generate videos
  • --log: log the results to a file
  • --dpi: dpi of the video
  • --nojit-rollout: do not use jit to speed up the rollout, used for large-scale tests

To test the nominal controller, use:

algo: 1.centralized_cbf 2.dec_share_cbf 3.gcbf_plus 4.gcbf

python test.py --env SingleIntegrator -n 100 --u-ref --epi 1 --area-size 4 --obs 20 --max-step 2000

To test the CBF-QPs, use:

python test.py --env CrazyFlie -n 32 --algo dec_share_cbf --area-size "(4,4)" --alpha 1 --max-step 4096 --epi 4 --no-video --log --gpu 2

python test.py --env CrazyFlie -n 16 --algo centralized_cbf --area-size "(4,4)" --alpha 1 --max-step 4096 --epi 4 --no-video --log

To test the GCBF+,use:

dubins训练需要修改：

  1. dubins_car.py中相关维度和state变化
  2. data_collector.py中初始化env的维度 和 收集的训练数据对state进行处理
  3. Dkoopman_affine_with_control.py中对模型进行测试时，test函数需要对state进行处理
python test.py --env SingleIntegrator -n 10 --algo gcbf+ --epi 1 --area-size "(4,4)" --obs 8 --alpha 1 --max-step 1000 --step 1000 --path "pretrained/SingleIntegrator/gcbf+" 

python test.py --env DubinsCar -n 2 --algo gcbf+ --epi 1 --area-size 4 --obs 10 --alpha 1 --max-step 1000 --path "pretrained/DubinsCar/gcbf+"

python test.py --env DoubleIntegrator -n 40 --algo r_gcbf+ --epi 1 --area-size "(4,4)" --obs 10 --alpha 1 --max-step 1000 --step 1000 --path "pretrained/DoubleIntegrator/r_gcbf+" --add-noise-model --add-noise --no-video

python test.py --env DoubleIntegrator -n 4 --algo gcbf+ --epi 1 --area-size "(4,4)" --obs 4 --alpha 1 --max-step 1 --step 1000 --path "pretrained/DoubleIntegrator/gcbf+"  --no-video

python test.py --env DoubleIntegrator -n 8 --algo informarl  --area-size "(4,4)" --obs 4 --alpha 1 --max-step 1000 --step 1000 --path "pretrained/DoubleIntegrator/informarl"  --no-video

python test.py --env DoubleIntegrator -n 32 --algo informarl  --area-size "(8,8)" --alpha 1 --max-step 1000 --step 1000 --path "pretrained/DoubleIntegrator/informarl"  --no-video

python test.py --env CrazyFlie -n 8 --obs 8 --algo k_gcbf+ --epi 1 --area-size "(4,4)"  --alpha 1 --max-step 1000 --step 1000 --path "pretrained/DubinsCar/k_gcbf+" --use-koopman

python test_lqr.py --env DoubleIntegrator -n 1 --algo k_gcbf+ --epi 1 --area-size "(4,4)"  --alpha 1 --max-step 10 --step 1000 --path "pretrained/DoubleIntegrator/k_gcbf+" --no-video --use-koopman

python test_lqr.py --env CrazyFlie -n 1 --algo k_gcbf+ --epi 1 --area-size "(4,4)"  --alpha 1 --max-step 10 --step 1000 --path "pretrained/DoubleIntegrator/k_gcbf+"  --no-video --use-koopman

测试：

添加噪声本身的均值和方差 噪声模型的边界 主要测试安全率和成功率等指标

Experiment

source ~/lp/multitaction/devel/setup.bash
roslaunch turtle_control test.launch

source ~/yjq/catkin_ws/devel/setup.bash
roslaunch gcbfplus rviz.launch

roslaunch usb_cam usb_cam-test.launch
ROS_NAMESPACE=usb_cam rosrun image_proc image_proc
rosrun camera_proc demo.py

python experiment.py --env SingleIntegrator -n 8 --algo gcbf+ --epi 1 --obs 5 --robot-id "(2,3,5,6,7,8,9,10)" --alpha 1 --max-step 1000 --step 1000 --path "pretrained/SingleIntegrator/gcbf+"

rosbag record /rosout_agg /rosout /tuio_cur /tuio_obj /robot_0_control/v_cmd /robot_1_control/v_cmd /robot_2_control/v_cmd /robot_3_control/v_cmd /robot_4_control/v_cmd /robot_5_control/v_cmd /robot_6_control/v_cmd /robot_7_control/v_cmd /robot_8_control/v_cmd /robot_9_control/v_cmd /robot_10_control/v_cmd -O my_recording

python experiment.py --env SingleIntegrator -n 1 --algo gcbf+ --epi 1 --obs 3 --robot-id "(4)" --alpha 1 --max-step 1000 --step 1000 --path "pretrained/SingleIntegrator/gcbf+" --no-visualization

python bag_process.py --bag-file bag/no_collision.bag --num-agents 8 --robot-id "(2,3,5,6,7,8,9,10)" --obs 5 --offline --rr-collision-threshold 0.08 --ro-collision-threshold 0.04 --env SingleIntegrator --alpha 1 --max-step 1000 --step 1000 --path "pretrained/SingleIntegrator/gcbf+" --create-video

Pre-trained models

We provide the pre-trained models in the folder pretrained.

Acknowledgement

The developers were partially supported by MITRE during the project.

© 2024 MIT

© 2024 The MITRE Corporation

ipdb使用方法

常用命令：

  • n (next)：执行当前行，并停在下一行。
  • s (step)：进入当前行调用的函数。
  • c (continue)：继续执行，直到下一个断点。
  • q (quit)：退出调试器。
  • p (print)：打印变量的值，例如 p variable_name。
  • pp (pretty print)：以美化格式打印变量。
  • l (list)：显示当前代码行附近的代码。
  • h (help)：显示帮助信息，或者使用 h command 来获取特定命令的帮助。

断点管理：

  • b (break)：设置断点，例如 b line_number 或 b function_name。
  • cl (clear)：清除断点，例如 cl breakpoint_number。
  • b：显示所有断点。

查看和修改变量：

  • p variable：打印变量的值。
  • set variable = value：修改变量的值。

调用栈导航：

  • u (up)：向上移动到调用栈的上一级。
  • d (down)：向下移动到调用栈的下一级。
  • where (w)：显示当前的调用栈。
  • bt (backtrace)：显示完整的调用栈。

执行控制：

  • r (return)：继续执行，直到当前函数返回。
  • j (jump)：跳转到指定行（需谨慎使用）。

wandb使用方法

登录: wandb login

离线上传: wandb sync wandb/offline-run-***

说明

该版本在miniconda下的gcbfplus虚拟环境下运行,该虚拟环境为python 3.10.14版本,需要额外安装rospy等库.

当前版本为v1.0:

  1. 添加从多点触摸屏获取环境信息的代码. (参考朱鹏铭师兄Guiding_Vector_Field_SC代码) 完成
  2. 需要对三轮全向轮小车重新进行建模,完成状态空间方程等内容编写(可参考刘鹏论文) 基本完成,没有最终建模到三个轮速.若建模到三个轮速,则B矩阵需要每个step求雅可比矩阵.
  3. 限制输出幅度应该在最终网络输出层进行限制!! 完成
  4. 屏幕坐标系关于角度定义是否影响三轮小车具体控制问题 解决
  5. 具体实物实验，大概8个小车，障碍物若干 目前还需要可视化界面设计一下。

版本v1.1: 1.具体idea为能够实现任意数量的agent增减 环境障碍物其实不需要增减，因为使用的是激光雷达数据，而不是全局数据 具体还需要读一下论文确定一下。 增加agent数量解决，减少需要再补充。 2.未来可以增加小车急停程序 解决

idea:

  1. 解决多机器人死锁（解决不了，PASS） - 现状：文章提到当前框架中机器人之间没有合作机制，这可能导致保守行为甚至死锁。死锁通常发生在多个机器人互相阻挡，无法找到可行的路径时。 - 优化建议： - 引入优先级机制：可以为机器人分配优先级，优先级高的机器人优先通过，优先级低的机器人等待或绕行。这种方法可以在一定程度上缓解死锁问题。 - 全局路径规划与局部避障结合：虽然文章强调分布式控制，但在某些情况下，引入全局路径规划（如A*算法或RRT）可以帮助机器人提前规划路径，避免局部死锁。 - 死锁检测与恢复机制：可以设计一种死锁检测算法，当检测到死锁时，机器人可以临时改变目标或调整优先级，以打破死锁状态。

  2. 改进图神经网络（目前仍在思考，加入Graph Koopman Autoencoder？？） - 现状：文章使用图神经网络（GNN）来建模机器人之间的交互关系，并通过注意力机制来处理邻居节点的动态变化。然而，GNN的性能可能受到图结构复杂性和节点数量的限制。 - 优化建议： - 动态图神经网络（Dynamic GNN）：可以引入动态图神经网络，实时更新图结构，以适应机器人邻居的动态变化。这种方法可以更好地处理大规模系统中的复杂交互。 - 分层图神经网络：对于大规模系统，可以考虑分层图神经网络，将机器人分组处理，减少计算复杂度。每组机器人可以有一个局部GNN，而全局GNN则处理组与组之间的交互。 - 图注意力机制改进：当前的注意力机制主要用于邻居节点的权重分配，可以进一步改进为多头注意力机制，以捕捉不同层次的交互信息。

  3. 考虑多机器人之间的合作（得考虑强化学习了） - 现状：文章中的机器人是独立决策的，没有考虑机器人之间的合作。这可能导致保守行为或效率低下。 - 优化建议： - 分布式合作机制：可以引入分布式合作机制，机器人之间通过通信共享信息（如目标位置、速度等），并协调行动。例如，机器人可以通过协商分配任务或共享路径信息。 - 任务分配与协同控制：可以引入任务分配算法（如拍卖算法或匈牙利算法），将任务分配给最适合的机器人，并通过协同控制算法（如分布式模型预测控制）来协调机器人的行动。 - 强化学习中的合作策略：可以引入多智能体强化学习（MARL）中的合作策略，机器人通过共享奖励信号来学习合作行为，从而提高整体系统的效率。

  4. 考虑机器人运动学噪声，改进算法的鲁棒性（RCBF、高斯噪声、高斯回归(GP)） - 现状：文章假设机器人的状态信息是精确的，但在实际应用中，机器人可能会受到传感器噪声、执行器误差等影响，导致状态估计不准确。 - 优化建议： - 鲁棒控制屏障函数（Robust CBF）：可以引入鲁棒控制屏障函数，考虑机器人状态的不确定性，确保在存在噪声的情况下仍能保证安全性。鲁棒CBF可以通过引入不确定性边界来处理状态估计误差。 - 卡尔曼滤波或粒子滤波：可以在机器人状态估计中引入卡尔曼滤波或粒子滤波，以减少传感器噪声对状态估计的影响。这可以提高控制算法的鲁棒性。 - 自适应控制：可以引入自适应控制算法，实时调整控制策略以应对系统动态变化和噪声。自适应控制可以通过在线学习来调整控制参数，确保系统在不确定环境中的稳定性。

About

No description, website, or topics provided.

Resources

Readme

License

MIT license
Activity

Stars

1 star

Watchers

1 watching

Forks

0 forks
Report repository

Releases

No releases published

Packages

No packages published

Languages