python-motion-planning 2.0.dev1__py3-none-any.whl → 2.0.1__py3-none-any.whl

{python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.1.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: python-motion-planning
-Version: 2.0.dev1
+Version: 2.0.1
 Summary: Motion planning algorithms for Python
 Maintainer-email: Wu Maojia <omige@mail.nwpu.edu.cn>, Yang Haodong <913982779@qq.com>
 License: GNU GENERAL PUBLIC LICENSE
@@ -685,15 +685,21 @@ Requires-Python: >=3.6
 Description-Content-Type: text/markdown
 License-File: LICENSE
 Requires-Dist: numpy
+Requires-Dist: numba
 Requires-Dist: scipy
 Requires-Dist: matplotlib
 Requires-Dist: osqp
 Requires-Dist: gymnasium
+Requires-Dist: faiss-cpu
+Requires-Dist: pyvista
+Requires-Dist: pyvistaqt
 Dynamic: license-file
 
 
 # Introduction
 
+`Python Motion Planning` repository provides the implementations of common `Motion planning` algorithms, including path planners on N-D grid, controllers for path-tracking, trajectory optimizers, a visualizer based on matplotlib and a toy physical simulator to test controllers.
+
 `Motion planning` plans the state sequence of the robot without conflict between the start and goal. 
 
 `Motion planning` mainly includes `Path planning` and `Trajectory planning`.
@@ -705,7 +711,7 @@ The theory analysis can be found at [motion-planning](https://blog.csdn.net/frig
 
 We also provide [ROS C++](https://github.com/ai-winter/ros_motion_planning) version and [Matlab](https://github.com/ai-winter/matlab_motion_planning) version.
 
-This repository provides the implementations of common `Motion planning` algorithms. **Your stars and forks are welcome**. Submitting pull requests or joining our development team are also welcome. For trivial modification, please directly contribute to `dev` branch. For big modification, please [contact](#contact) us before you contribute.
+**Your stars and forks are welcome!**
 
 # Quick Start
 
@@ -725,8 +731,10 @@ python_motion_planning
 |   └─path_tracker
 ├─path_planner
 |   ├─graph_search
-|   └─sample_search
-└─curve_generation
+|   ├─sample_search
+|   └─hybrid_search
+└─traj_optimizer
+    └─curve_generator
 ```
 
 ## Install
@@ -740,7 +748,7 @@ conda activate pmp
 To install the repository, please run the following command in shell.
 
 ```shell
-pip install python-motion-planning==2.0.dev1
+pip install python-motion-planning
 ```
 
 ## Run
@@ -749,29 +757,40 @@ Please refer to the Tutorials part of [online documentation](https://ai-winter.g
 
 # Demos
 ## Path Planner
-
+### Graph Search
 |Planner|2D Grid|3D Grid
 |-------|-------|-------
-**GBFS**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**Dijkstra**|![dijkstra_2d.svg](assets/dijkstra_2d.svg)|![dijkstra_3d.svg](assets/dijkstra_3d.svg)
-**A\***|![a_star_2d.svg](assets/a_star_2d.svg)|![a_star_3d.svg](assets/a_star_3d.svg)
-**JPS**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
+**Dijkstra**|![dijkstra_2d.svg](assets/dijkstra_2d.svg)|![dijkstra_3d.png](assets/dijkstra_3d.png)
+**GBFS**|![gbfs_2d.svg](assets/gbfs_2d.svg)|![gbfs_3d.png](assets/gbfs_3d.png)
+**A\***|![a_star_2d.svg](assets/a_star_2d.svg)|![a_star_3d.png](assets/a_star_3d.png)
+**JPS**|![jps_2d.svg](assets/jps_2d.svg)|![jps_3d.png](assets/jps_3d.png)
+**Theta\***|![theta_star_2d.svg](assets/theta_star_2d.svg)|![theta_star_3d.png](assets/theta_star_3d.png)
+**Lazy Theta\***|![lazy_theta_star_2d.svg](assets/lazy_theta_star_2d.svg)|![lazy_theta_star_3d.png](assets/lazy_theta_star_3d.png)
 **D\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **LPA\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **D\* Lite**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**Theta\***|![theta_star_2d.svg](assets/theta_star_2d.svg)|![theta_star_3d.svg](assets/theta_star_3d.svg)
-**Lazy Theta\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**S-Theta\***|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 **Anya**|Not implemented|Not implemented
-**Voronoi**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**RRT**|![rrt_2d.svg](assets/rrt_2d.svg)|![rrt_3d.svg](assets/rrt_3d.svg)
-**RRT\***|![rrt_star_2d.svg](assets/rrt_star_2d.svg)|![rrt_star_3d.svg](assets/rrt_star_3d.svg)
+
+### Sample Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
+**RRT**|![rrt_2d.svg](assets/rrt_2d.svg)|![rrt_3d.png](assets/rrt_3d.png)
+**RRT\***|![rrt_star_2d.svg](assets/rrt_star_2d.svg)|![rrt_star_3d.png](assets/rrt_star_3d.png)
+**RRT-Connect**|![rrt_connect_2d.svg](assets/rrt_connect_2d.svg)|![rrt_connect_3d.png](assets/rrt_connect_3d.png)
 **Informed RRT**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
-**RRT-Connect**|Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
+**PRM**|Not implemented|Not implemented
+
+### Evolutionary Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
 | **ACO** |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **GA**  |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **PSO**  |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 
+### Hybrid Search
+|Planner|2D Grid|3D Grid
+|-------|-------|-------
+**Voronoi Planner**|![voronoi_planner_2d.svg](assets/voronoi_planner_2d.svg)|![voronoi_planner_3d.png](assets/voronoi_planner_3d.png)
 
 ## Controller
 
@@ -779,14 +798,14 @@ We provide a toy simulator with simple physical simulation to test controllers (
 
 In the following demos, the blue robot 1 is the `CircularRobot`, and the orange robot 2 is the `DiffDriveRobot`.
 
-|Planner|2D|3D
+|Controller|2D|3D
 |-------|-------|-------
 |**Path Trakcer**|![path_tracker_2d.gif](assets/path_tracker_2d.gif)|Not implemented
 | **Pure Pursuit**     |![pure_pursuit_2d.gif](assets/pure_pursuit_2d.gif)|Not implemented
 | **PID**     |![pid_2d.gif](assets/pid_2d.gif)|Not implemented
 | **APF**     |![apf_2d.gif](assets/apf_2d.gif)|Not implemented
 | **DWA**     |![dwa_2d.gif](assets/dwa_2d.gif)|Not implemented
-| **RPP**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
+| **RPP**     |![rpp_2d.gif](assets/rpp_2d.gif)|Not implemented
 | **LQR**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPC**     |Implemented in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1), not migrated|Not implemented
 | **MPPI**    |Not implemented|Not implemented
@@ -795,19 +814,42 @@ In the following demos, the blue robot 1 is the `CircularRobot`, and the orange
 | **DQN**    |Not implemented|Not implemented
 | **DDPG**    |Implemented in [V1.0](https://github.com/ai-winter/python_motion_planning/tree/v1.0), not migrated|Not implemented
 
-## Curve Generator
+## Trajectory Optimizer
+### Curve Generator
+#### Point-based
 
-The visualization of the curve generators has not been implemented in current version. They can be visualized in [V1.1.1](https://github.com/ai-winter/python_motion_planning/tree/v1.1.1).
+|Generator|2D|3D|
+| ------- | -------------------------------------------------------- | -------------------------------------------------------- 
+| **Cubic Spline** |![cubic_spline_2d.svg](assets/cubic_spline_2d.svg)|Not implemented
+| **BSpline** |![bspline_2d.svg](assets/bspline_2d.svg)|Not implemented
 
-| Planner |2D|3D|
+#### Pose-based
+|Generator|2D|3D|
 | ------- | -------------------------------------------------------- | -------------------------------------------------------- 
-| **Polynomia** | ![polynomial_curve_python.gif](assets/polynomial_curve_python.gif)|Not implemented
-| **Bezier** |![bezier_curve_python.png](assets/bezier_curve_python.png)|Not implemented
-| **Cubic Spline** |![cubic_spline_python.png](assets/cubic_spline_python.png)|Not implemented
-| **BSpline** |![bspline_curve_python.png](assets/bspline_curve_python.png)|Not implemented
-| **Dubins** |![dubins_curve_python.png](assets/dubins_curve_python.png)|Not implemented
-| **Reeds-Shepp** |![reeds_shepp_python.png](assets/reeds_shepp_python.gif)|Not implemented
-| **Fem-Pos Smoother** |![fem_pos_smoother_python.png](assets/fem_pos_smoother_python.png)|Not implemented
+| **Polynomia** | ![polynomial_2d.svg](assets/polynomial_2d.svg)|Not implemented
+| **Bezier** |![bezier_2d.svg](assets/bezier_2d.svg)|Not implemented
+| **Dubins** |![dubins_2d.svg](assets/dubins_2d.svg)|Not implemented
+| **Reeds-Shepp** |![reeds_shepp_2d.svg](assets/reeds_shepp_2d.svg)|Not implemented
+
+# Future Works
+
+* N-D controllers (path-trackers).
+
+* Path planning for robotic arms.
+
+* Path planning on topological map.
+
+* Sample search with Dubins or Reeds-Shepp curves.
+
+* Application on ROS2.
+
+* Application in mainstream robot simulation environments (e.g. Gazebo, Carla, Airsim, PyBullet, MuJoCo, Issac Sim).
+
+* More mainstream motion planning algorithms.
+
+* Performance optimization.
+
+Contributors are welcome! For trivial modification, please directly contribute to `dev` branch. For big modification, please [contact](#contact) us before you contribute.
 
 # Contact
 

python_motion_planning-2.0.1.dist-info/RECORD

@@ -0,0 +1,64 @@
+python_motion_planning/__init__.py,sha256=qsRVlO3n-WSdeyH6NvduCQJ_G0Yqrml7F_juvs2UnJ4,108
+python_motion_planning/common/__init__.py,sha256=9CeruPgGOBOtro9EafPEhK5VKTav2OWeNbMLJCBwuNg,67
+python_motion_planning/common/env/__init__.py,sha256=Q8qIUcn5fvtLrmrc3SXStQoEwwrNMF-IofKotFermBk,129
+python_motion_planning/common/env/node.py,sha256=6I10JpU4tjK4aZEMWN7Dttz41QKskNFoe7gBR4aSyII,2727
+python_motion_planning/common/env/types.py,sha256=h5HtPrffu5qH_F1G4V1g8eXBPTPuDW-nFFfEFECVGYo,380
+python_motion_planning/common/env/map/__init__.py,sha256=5bVsJd2mrghKMj7erXfgc8V4NahXHe6zXngG9fMCjo0,44
+python_motion_planning/common/env/map/base_map.py,sha256=tX-9iO3RwbdvhGUHDtPNQUj8h-HNONjopXCLyUdtfQE,3321
+python_motion_planning/common/env/map/grid.py,sha256=-QDhKpKrjE4Hi-NZPWIgPiiZagJCZH_dw8QgpHgLor4,28441
+python_motion_planning/common/env/robot/__init__.py,sha256=njq1_k2uWikGH2AjtxoFDdmxqsM1YY5B9nVtIheyx6Y,122
+python_motion_planning/common/env/robot/base_robot.py,sha256=KEajU9iFCnrzDOFp3Vm3SMWJWQgGJAYLT4VB6IMX2Is,7913
+python_motion_planning/common/env/robot/circular_robot.py,sha256=6SS3XP91WfQlccxbtyVLjmPQjgesP9cFaxXg6txkxVI,1512
+python_motion_planning/common/env/robot/diff_drive_robot.py,sha256=bJgaa7sheCb62ee891vEV3KigeX4nUOtec0iw2F7Syw,4389
+python_motion_planning/common/env/world/__init__.py,sha256=nC9YDBuO-8FapouUCe_relatAferP_gcgPkVQ3QCVNA,39
+python_motion_planning/common/env/world/base_world.py,sha256=FSf9PQAFY6CFgCSGK8TNdGDR8_GrIEtBW8CA83jWtEo,4131
+python_motion_planning/common/env/world/toy_simulator.py,sha256=J2JrZhQD-lCd4TB2X6O_wyX_NR_rIz3UjSzPrjyINEQ,9730
+python_motion_planning/common/utils/__init__.py,sha256=wYzKTiyKgYfl5cTO0pUHF1zkr8Q9VhhEwKcRy1xSSdM,114
+python_motion_planning/common/utils/child_tree.py,sha256=f6ypaZ-d7mJpWOaVH99KriJuzjDc6q353USsO7D5F9g,625
+python_motion_planning/common/utils/frame_transformer.py,sha256=D9xaOUcxCPLvTl9oJHRSuhue84i88nMiuI-3ns0i7Ao,7609
+python_motion_planning/common/utils/geometry.py,sha256=FFYM_eVAgJ3wA7H8MRXNvwXliGd6dKo4WU8fzbwdCXE,2693
+python_motion_planning/common/visualizer/__init__.py,sha256=HWWbLehtqEl0q_hEO28tEMDH-v8lXqduN-EJ7mo9LEI,90
+python_motion_planning/common/visualizer/base_visualizer.py,sha256=sjkH8lbyW8q9BVIEaQBTF10M27d73cpQqdTAPbEBFD0,5518
+python_motion_planning/common/visualizer/visualizer_2d.py,sha256=mqaKFZEtNHOCm82I1nCbCOhDP444JSySq1BzQCnPyEM,15656
+python_motion_planning/common/visualizer/visualizer_3d.py,sha256=4LklgQv9Wu7UT8hmJMLiGau_8GzrGcJpQ4Xj40m1rTA,7645
+python_motion_planning/controller/__init__.py,sha256=UGFx5oND_pyovs1LIZ2HYU8Toxo0QnkTSnqKITfX28M,121
+python_motion_planning/controller/base_controller.py,sha256=bSeCXHM10KosvZmGhjHyUOoC-BvxdDWW_Q2IeyGJ-yQ,6890
+python_motion_planning/controller/random_controller.py,sha256=al9Az3K4K8HlfQ-BQNcoS-1T6O2v1-juu8Fw5lL3dEE,693
+python_motion_planning/controller/path_tracker/__init__.py,sha256=1giXD5W8aXdTc1H_DO9P0MK8JjC6Jb_tsnrT3Gz4JDM,164
+python_motion_planning/controller/path_tracker/apf.py,sha256=wV_OiPua5djMwP_i8eZP0FX8e3Y_kov-HsMOmbRGtFE,9531
+python_motion_planning/controller/path_tracker/dwa.py,sha256=EyjXdU_lw_hIN6lucfaokxx8C1OxPCLk13zmcy7KLR8,9495
+python_motion_planning/controller/path_tracker/path_tracker.py,sha256=5p_MhqRWpMGEsdek0IqcohmXCMTlStUrMwSoJKX14zw,9359
+python_motion_planning/controller/path_tracker/pid.py,sha256=pqZggmTWaNd7XrlqfaK4wtBk9syg8PnIC7vOEnfN9S8,2403
+python_motion_planning/controller/path_tracker/pure_pursuit.py,sha256=gsfiMXZx3RwHIwmI2CSu4fTZNFKbZP31cXMgKUdDjs8,2405
+python_motion_planning/controller/path_tracker/rpp.py,sha256=e7qRIaUfeiVfVoiY5k797Yk9RK-Q7gnegmaOB7_BVP4,4181
+python_motion_planning/path_planner/__init__.py,sha256=Y0Im0cJ3iuUOYJyAOYvA_IavaNTNowb7SE6atrdJqX0,121
+python_motion_planning/path_planner/base_path_planner.py,sha256=YZcLqAIkLJtp5MWoT5zPmzxv05xUlnDpfuzN-CgPQuc,3640
+python_motion_planning/path_planner/graph_search/__init__.py,sha256=ugZpHV12dY-yb8d1jx4NepdRzQKn2qNKS6yWEBlvWy0,146
+python_motion_planning/path_planner/graph_search/a_star.py,sha256=s_PLSsF53xcvVM5DOnFDCW5Enp4TrkL1keuPuKWIEEA,2989
+python_motion_planning/path_planner/graph_search/dijkstra.py,sha256=Z3f2KuR2dQ2VAc1jcScD9Qo05qFVUULHhSz58QlHQEk,3169
+python_motion_planning/path_planner/graph_search/gbfs.py,sha256=S9MSwjNavvSiau5sQJRum-Nq0GYInFa6SmfzJfWdxNw,3261
+python_motion_planning/path_planner/graph_search/jps.py,sha256=AOfGLBmSS1HMX_XjvpQKGLH3-e80MwzLGKG0_afXWrE,7657
+python_motion_planning/path_planner/graph_search/lazy_theta_star.py,sha256=Gyu8F16lEqaqfZteRwYeu1UkhWQRnWL0wSJbM7H1oWw,3965
+python_motion_planning/path_planner/graph_search/theta_star.py,sha256=JxxWukRTI4aZWF9pppzpsjqwRh6A6gvfB5YLZIk45w0,3846
+python_motion_planning/path_planner/hybrid_search/__init__.py,sha256=RCvNhSq6nskZPCc7jo9Y5MOzrD4f7TL9RfVUCHgpeq4,30
+python_motion_planning/path_planner/hybrid_search/voronoi_planner.py,sha256=sDTE-IFMf2035mUIRulEIOF0NiZSrwhW_bWrCiPetu4,7897
+python_motion_planning/path_planner/sample_search/__init__.py,sha256=RYz6o_xmdsoErMGdBKvXEK4w-48P_ogi1bJsdJmUupk,71
+python_motion_planning/path_planner/sample_search/rrt.py,sha256=hFBN1RaAnBn_ap7V2IPdkkKJIaMEIc7bHGALxoMkV-I,8682
+python_motion_planning/path_planner/sample_search/rrt_connect.py,sha256=2GYQS5InB-jWr74c_dDGnLLCkIIeLnIrJmgfBfNNtOw,9670
+python_motion_planning/path_planner/sample_search/rrt_star.py,sha256=f32G0h3_ds0cWvukNDRtDUoLZy0_KYpALcqCQcTxP1w,11223
+python_motion_planning/traj_optimizer/__init__.py,sha256=0HeRril2pkNw1uUdro2HuV5z3eouQaC-kh6_uHOTzZg,69
+python_motion_planning/traj_optimizer/base_curve_generator.py,sha256=m7-EZOIH5lOFiMVyvh5LYemjnWsiw0yRAvYUWG5mIFQ,1619
+python_motion_planning/traj_optimizer/curve_generator/__init__.py,sha256=q40AJi09Wgv84NEU7fkdcSdNIGwoKy42Bsnt7YHaOdY,55
+python_motion_planning/traj_optimizer/curve_generator/point_based/__init__.py,sha256=3V3ddQ0RfWs9oS57MUioGqzmMhPEXdVxbHkL9t_KlSM,53
+python_motion_planning/traj_optimizer/curve_generator/point_based/bspline.py,sha256=GRcRHCC56Kdm74LFv96ANzHdaBxihTCfL-sk87rD3h8,9110
+python_motion_planning/traj_optimizer/curve_generator/point_based/cubic_spline.py,sha256=sArmL1kLKHc-NRLFtczwpO6ATmuI3SB_FHTgSk826wI,3962
+python_motion_planning/traj_optimizer/curve_generator/pose_based/__init__.py,sha256=muLe-_uENMf_btwqmVsRP0Ikbryut3Wl0_0BxNRhhbU,101
+python_motion_planning/traj_optimizer/curve_generator/pose_based/bezier.py,sha256=BuLp6MzbyhuElD4qnos_nQQi9HvRVxx2AI5xPuUf2_I,4405
+python_motion_planning/traj_optimizer/curve_generator/pose_based/dubins.py,sha256=76ujAOJHl3o4HkV1e5wuZEyz9wxzaGkDizDUx3M9C0w,13895
+python_motion_planning/traj_optimizer/curve_generator/pose_based/polynomial.py,sha256=kjDkVskkfFHERZdJx6GU_GrRTJmrGHuX383W2s1oECI,7381
+python_motion_planning/traj_optimizer/curve_generator/pose_based/reeds_shepp.py,sha256=I6KjYZvG4lOE2N9_vp1hSzJuan4SOrpuQF2L57owII4,22970
+python_motion_planning-2.0.1.dist-info/licenses/LICENSE,sha256=a4N8el8H0UrdAJzCeVgj9HklpT3VkZfsy4aL1SAifJE,35793
+python_motion_planning-2.0.1.dist-info/METADATA,sha256=61r3x6QoQCBJIHm1yt10khJLLdTw5P7zEXHZGkZwr7Y,49587
+python_motion_planning-2.0.1.dist-info/WHEEL,sha256=aeYiig01lYGDzBgS8HxWXOg3uV61G9ijOsup-k9o1sk,91
+python_motion_planning-2.0.1.dist-info/top_level.txt,sha256=VtZTYZios8VncYIkec7tOVdYqj5oMRmoeBb9RcJHluY,23
+python_motion_planning-2.0.1.dist-info/RECORD,,

{python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.1.dist-info}/WHEEL

@@ -1,5 +1,5 @@
 Wheel-Version: 1.0
-Generator: setuptools (80.9.0)
+Generator: setuptools (82.0.1)
 Root-Is-Purelib: true
 Tag: py3-none-any
 

python_motion_planning/common/env/robot/tmp.py

@@ -1,404 +0,0 @@
-"""
-multiagent_nav.py
-
-依赖: gymnasium, numpy, matplotlib
-
-保存并运行后，可通过 demo_2d() / demo_3d() 运行演示。
-"""
-
-from typing import List, Dict, Tuple, Optional
-import numpy as np
-import gymnasium as gym
-from gymnasium import spaces
-import math
-import matplotlib.pyplot as plt
-from matplotlib import animation
-from mpl_toolkits.mplot3d import Axes3D  # noqa: F401
-
-# ---------------------------
-# Base classes: Agent / Controller / Environment
-# ---------------------------
-
-class BaseAgent:
-    """
-    Agent 基类：只包含物理参数和状态容器，负责定义观测格式。
-    - dim: 空间维度（N）
-    - mass: 质量
-    - radius: 形状半径（用于碰撞）
-    - pos, vel: 当前状态（numpy arrays）
-    - acc: 当前瞬时加速度（set by controller before env.step 使用）
-    """
-    def __init__(self, dim: int = 2, mass: float = 1.0, radius: float = 0.1,
-                 pos: Optional[np.ndarray] = None, vel: Optional[np.ndarray] = None,
-                 action_min: Optional[np.ndarray] = None, action_max: Optional[np.ndarray] = None):
-        self.dim = dim
-        self.mass = float(mass)
-        self.radius = float(radius)
-        self.pos = np.zeros(dim) if pos is None else np.array(pos, dtype=float)
-        self.vel = np.zeros(dim) if vel is None else np.array(vel, dtype=float)
-        # acceleration is set externally by controller each step
-        self.acc = np.zeros(dim)
-        # action bounds per-dim (controller output bounds)
-        if action_min is None:
-            action_min = -np.ones(dim) * 1.0
-        if action_max is None:
-            action_max = np.ones(dim) * 1.0
-        self.action_min = np.array(action_min, dtype=float)
-        self.action_max = np.array(action_max, dtype=float)
-
-    def observation_size(self, env) -> int:
-        """
-        默认观测：自身 pos, vel (2*dim) + 所有其他 agent 的相对位置 ( (n-1)*dim )
-        你可以重载该函数改变观测结构。
-        """
-        n_agents = len(env.agents)
-        return 2 * self.dim + (n_agents - 1) * self.dim
-
-    def get_observation(self, env) -> np.ndarray:
-        """
-        返回观测向量（1D numpy array）。
-        默认格式： [pos, vel, rel_pos_agent1, rel_pos_agent2, ...]
-        相对位置按照 env.agents 列表顺序（跳过 self）。
-        """
-        obs = []
-        obs.extend(self.pos.tolist())
-        obs.extend(self.vel.tolist())
-        for a in env.agents:
-            if a is self:
-                continue
-            rel = (a.pos - self.pos)
-            obs.extend(rel.tolist())
-        return np.array(obs, dtype=float)
-
-    def clip_action(self, a: np.ndarray) -> np.ndarray:
-        return np.minimum(np.maximum(a, self.action_min), self.action_max)
-
-
-class SimpleAgent(BaseAgent):
-    """一个简单的圆/球体 agent（继承 BaseAgent）。"""
-    pass
-
-
-class BaseController:
-    """
-    Controller 基类（与环境解耦）
-    - 控制器只知道：observation_space, action_space（由 agent 定义）
-    - get_action(obs) -> acceleration vector (dim,)
-    """
-    def __init__(self, observation_space: spaces.Space, action_space: spaces.Box):
-        self.observation_space = observation_space
-        self.action_space = action_space
-
-    def reset(self):
-        """可选：用于在每个 episode 开始时清理内部状态。"""
-        pass
-
-    def get_action(self, obs: np.ndarray) -> np.ndarray:
-        """子类必须实现：返回与 action_space.shape 匹配的 ndarray（加速度）"""
-        raise NotImplementedError
-
-
-class RandomController(BaseController):
-    """默认随机控制器：在 action_space 内均匀采样。"""
-    def get_action(self, obs: np.ndarray) -> np.ndarray:
-        return self.action_space.sample()
-
-
-# ---------------------------
-# Environment
-# ---------------------------
-
-class MultiAgentNavEnv(gym.Env):
-    """
-    多智能体导航环境（可 N 维）
-    - agents: list of BaseAgent 的实例
-    - bounds: environment boundary as (min_vec, max_vec) each of length dim
-    - dt: 时间步长
-    - friction: 线性阻尼系数（v * (-friction) force），模型里转成加速度影响
-    - restitution: 边界/碰撞弹性系数 [0,1]
-    - max_episode_steps: 可选终止步数
-    """
-    metadata = {"render.modes": ["human"]}
-
-    def __init__(self, dim: int = 2,
-                 bounds: Tuple[np.ndarray, np.ndarray] = None,
-                 dt: float = 0.05,
-                 friction: float = 0.1,
-                 restitution: float = 0.9,
-                 max_episode_steps: int = 1000):
-        super().__init__()
-        self.dim = dim
-        if bounds is None:
-            lo = -np.ones(dim) * 5.0
-            hi = np.ones(dim) * 5.0
-            bounds = (lo, hi)
-        self.bounds = (np.array(bounds[0], dtype=float), np.array(bounds[1], dtype=float))
-        self.dt = float(dt)
-        self.friction = float(friction)
-        self.restitution = float(restitution)
-        self.max_episode_steps = int(max_episode_steps)
-
-        self.agents: List[BaseAgent] = []
-        self.step_count = 0
-        # observation_space and action_space are per-agent; environment doesn't expose global spaces
-        # 但为了兼容 gym，提供 a dummy space
-        self.observation_space = spaces.Box(-np.inf, np.inf, shape=(1,), dtype=float)
-        self.action_space = spaces.Box(-np.inf, np.inf, shape=(1,), dtype=float)
-
-    def add_agent(self, agent: BaseAgent):
-        if agent.dim != self.dim:
-            raise ValueError("Agent dimension must match environment dimension")
-        self.agents.append(agent)
-
-    def reset(self, seed: Optional[int] = None):
-        self.step_count = 0
-        # optionally randomize initial states or rely on agents' initial pos/vel
-        # 返回 dict of observations keyed by agent index
-        obs = {}
-        for i, a in enumerate(self.agents):
-            obs[i] = a.get_observation(self)
-        return obs, {}
-
-    def step(self, actions: Dict[int, np.ndarray]):
-        """
-        actions: dict mapping agent_index -> acceleration ndarray (dim,)
-        1) clip to agent action bounds
-        2) apply environment forces (friction) and integrate via semi-implicit Euler
-        3) handle collisions (agent-agent, agent-boundary)
-        返回：obs_dict, reward_dict, done_dict, info
-        """
-        self.step_count += 1
-        # 1. assign actions (accelerations) to agents
-        for i, a in enumerate(self.agents):
-            act = actions.get(i, np.zeros(self.dim))
-            a.acc = a.clip_action(np.array(act, dtype=float))
-
-        # 2. apply environment forces -> compute net acceleration: a_net = a.acc + a_env (friction)
-        for a in self.agents:
-            # friction as linear damping: a_fric = -friction * v / mass
-            a_env_acc = - self.friction * a.vel / (a.mass + 1e-12)
-            a_net = a.acc + a_env_acc
-            # semi-implicit Euler: v += a_net*dt, pos += v*dt
-            a.vel = a.vel + a_net * self.dt
-            a.pos = a.pos + a.vel * self.dt
-
-        # 3. collisions: pairwise agent-agent elastic collisions (simple impulse) and boundary collisions
-        self._resolve_agent_collisions()
-        self._resolve_boundary_collisions()
-
-        obs = {i: a.get_observation(self) for i, a in enumerate(self.agents)}
-        # no rewards by default; you can extend
-        rewards = {i: 0.0 for i in range(len(self.agents))}
-        dones = {i: False for i in range(len(self.agents))}
-        terminated = False
-        truncated = self.step_count >= self.max_episode_steps
-        info = {}
-        return obs, rewards, dones, {"terminated": terminated, "truncated": truncated, **info}
-
-    def _resolve_agent_collisions(self):
-        n = len(self.agents)
-        for i in range(n):
-            for j in range(i + 1, n):
-                a = self.agents[i]
-                b = self.agents[j]
-                delta = b.pos - a.pos
-                dist = np.linalg.norm(delta)
-                min_dist = a.radius + b.radius
-                if dist < 1e-8:
-                    # numeric edge-case: jitter them slightly
-                    delta = np.random.randn(self.dim) * 1e-6
-                    dist = np.linalg.norm(delta)
-                if dist < min_dist:
-                    # push them apart and compute elastic collision impulse
-                    # normal vector
-                    nvec = delta / dist
-                    # relative velocity along normal
-                    rel_vel = np.dot(b.vel - a.vel, nvec)
-                    # compute impulse scalar (elastic) with restitution
-                    e = self.restitution
-                    j_impulse = -(1 + e) * rel_vel / (1 / a.mass + 1 / b.mass)
-                    if j_impulse < 0:
-                        # apply impulse
-                        a.vel = a.vel - (j_impulse / a.mass) * nvec
-                        b.vel = b.vel + (j_impulse / b.mass) * nvec
-                    # positional correction (simple)
-                    overlap = min_dist - dist
-                    corr = nvec * (overlap / 2.0 + 1e-6)
-                    a.pos = a.pos - corr
-                    b.pos = b.pos + corr
-
-    def _resolve_boundary_collisions(self):
-        lo, hi = self.bounds
-        for a in self.agents:
-            for d in range(self.dim):
-                if a.pos[d] - a.radius < lo[d]:
-                    a.pos[d] = lo[d] + a.radius
-                    if a.vel[d] < 0:
-                        a.vel[d] = -a.vel[d] * self.restitution
-                elif a.pos[d] + a.radius > hi[d]:
-                    a.pos[d] = hi[d] - a.radius
-                    if a.vel[d] > 0:
-                        a.vel[d] = -a.vel[d] * self.restitution
-
-    def render(self, mode="human", ax=None):
-        # delegated to demo functions; keep signature for Gym compatibility
-        raise NotImplementedError("render(): use provided demo_2d/demo_3d functions for visualization.")
-
-    def close(self):
-        pass
-
-    # helper to build spaces per-agent
-    def build_agent_spaces(self, agent: BaseAgent) -> Tuple[spaces.Box, spaces.Box]:
-        """
-        返回 (observation_space, action_space) for given agent
-        observation_space: shape (observation_size,)
-        action_space: shape (dim,) bounded by agent.action_min / action_max
-        """
-        obs_dim = agent.observation_size(self)
-        obs_low = -np.inf * np.ones(obs_dim)
-        obs_high = np.inf * np.ones(obs_dim)
-        obs_space = spaces.Box(obs_low, obs_high, dtype=float)
-        act_low = agent.action_min
-        act_high = agent.action_max
-        act_space = spaces.Box(act_low, act_high, dtype=float)
-        return obs_space, act_space
-
-# ---------------------------
-# Demos: 2D & 3D visualization
-# ---------------------------
-
-def demo_2d():
-    """
-    2D 演示：生成 N 个圆形 agent，RandomController 控制，使用 matplotlib 动画绘制。
-    """
-    dim = 2
-    env = MultiAgentNavEnv(dim=dim, bounds=(np.array([-5, -5]), np.array([5, 5])),
-                           dt=0.03, friction=0.2, restitution=0.8, max_episode_steps=1000)
-
-    # add agents with different masses/radii and action limits per-dim
-    agents = [
-        SimpleAgent(dim=dim, mass=1.0, radius=0.25, pos=np.array([-3.0, -3.0]), vel=np.zeros(dim),
-                    action_min=np.array([-2.0, -2.0]), action_max=np.array([2.0, 2.0])),
-        SimpleAgent(dim=dim, mass=2.0, radius=0.35, pos=np.array([3.0, -3.0]), vel=np.zeros(dim),
-                    action_min=np.array([-1.5, -1.5]), action_max=np.array([1.5, 1.5])),
-        SimpleAgent(dim=dim, mass=1.2, radius=0.2, pos=np.array([0.0, 3.0]), vel=np.zeros(dim),
-                    action_min=np.array([-3.0, -3.0]), action_max=np.array([3.0, 3.0])),
-    ]
-    for a in agents:
-        env.add_agent(a)
-
-    # build controllers per-agent (Random by default)
-    controllers = []
-    for i, a in enumerate(env.agents):
-        obs_space, act_space = env.build_agent_spaces(a)
-        controllers.append(RandomController(obs_space, act_space))
-
-    obs, _ = env.reset()
-
-    # matplotlib setup
-    fig, ax = plt.subplots(figsize=(6, 6))
-    lo, hi = env.bounds
-    ax.set_xlim(lo[0], hi[0])
-    ax.set_ylim(lo[1], hi[1])
-    ax.set_aspect('equal')
-    patches = []
-    texts = []
-    colors = ['C0', 'C1', 'C2', 'C3', 'C4']
-    for i, a in enumerate(env.agents):
-        p = plt.Circle(tuple(a.pos), a.radius, color=colors[i % len(colors)], alpha=0.8)
-        patches.append(p)
-        ax.add_patch(p)
-        t = ax.text(*a.pos, f"{i}", color='white', ha='center', va='center')
-        texts.append(t)
-
-    # animation update
-    def update(frame):
-        actions = {}
-        for i, a in enumerate(env.agents):
-            ob = a.get_observation(env)
-            act = controllers[i].get_action(ob)
-            actions[i] = act
-        obs, rewards, dones, info = env.step(actions)
-        for i, a in enumerate(env.agents):
-            patches[i].center = tuple(a.pos)
-            texts[i].set_position(tuple(a.pos))
-        return patches + texts
-
-    ani = animation.FuncAnimation(fig, update, frames=400, interval=30, blit=False)
-    plt.title("Multi-Agent Navigation 2D Demo (Random Controllers)")
-    plt.show()
-
-
-def demo_3d():
-    """
-    3D 演示：类似 2D，但用 mpl_toolkits.mplot3d 展示球体（用 scatter 表示）。
-    注意：matplotlib 3D 动画较慢，但用于快速验证足够。
-    """
-    dim = 3
-    env = MultiAgentNavEnv(dim=dim, bounds=(np.array([-5, -5, -2]), np.array([5, 5, 2])),
-                           dt=0.05, friction=0.15, restitution=0.85, max_episode_steps=1000)
-
-    agents = [
-        SimpleAgent(dim=dim, mass=1.0, radius=0.25, pos=np.array([-3.0, -3.0, 0.0]),
-                    action_min=np.array([-2.0, -2.0, -1.0]), action_max=np.array([2.0, 2.0, 1.0])),
-        SimpleAgent(dim=dim, mass=1.5, radius=0.3, pos=np.array([3.0, -3.0, 0.5]),
-                    action_min=np.array([-1.5, -1.5, -1.0]), action_max=np.array([1.5, 1.5, 1.0])),
-        SimpleAgent(dim=dim, mass=0.8, radius=0.2, pos=np.array([0.0, 3.0, -0.5]),
-                    action_min=np.array([-3.0, -3.0, -1.0]), action_max=np.array([3.0, 3.0, 1.0])),
-    ]
-    for a in agents:
-        env.add_agent(a)
-
-    controllers = []
-    for i, a in enumerate(env.agents):
-        obs_space, act_space = env.build_agent_spaces(a)
-        controllers.append(RandomController(obs_space, act_space))
-
-    obs, _ = env.reset()
-
-    # matplotlib 3d setup
-    fig = plt.figure(figsize=(8, 6))
-    ax = fig.add_subplot(111, projection='3d')
-    lo, hi = env.bounds
-    ax.set_xlim(lo[0], hi[0])
-    ax.set_ylim(lo[1], hi[1])
-    ax.set_zlim(lo[2], hi[2])
-    scat = ax.scatter([a.pos[0] for a in env.agents],
-                      [a.pos[1] for a in env.agents],
-                      [a.pos[2] for a in env.agents],
-                      s=[(a.radius * 1000) for a in env.agents])
-
-    # annotation labels
-    annotations = [ax.text(a.pos[0], a.pos[1], a.pos[2], str(i)) for i, a in enumerate(env.agents)]
-
-    def update(frame):
-        actions = {}
-        for i, a in enumerate(env.agents):
-            ob = a.get_observation(env)
-            act = controllers[i].get_action(ob)
-            actions[i] = act
-        obs, rewards, dones, info = env.step(actions)
-        xs = [a.pos[0] for a in env.agents]
-        ys = [a.pos[1] for a in env.agents]
-        zs = [a.pos[2] for a in env.agents]
-        scat._offsets3d = (xs, ys, zs)
-        for i, ann in enumerate(annotations):
-            ann.set_position((xs[i], ys[i]))
-            # can't directly set z of text easily; remove & redraw would be heavy
-        return scat, *annotations
-
-    ani = animation.FuncAnimation(fig, update, frames=400, interval=50, blit=False)
-    plt.title("Multi-Agent Navigation 3D Demo (Random Controllers)")
-    plt.show()
-
-
-# ---------------------------
-# Example usage
-# ---------------------------
-
-if __name__ == "__main__":
-    # 运行 2D 或 3D demo
-    # print("Running 2D demo...")
-    demo_2d()
-    # 若要运行 3D，注释上两行，解除下一行注释：
-    # demo_3d()