chassis-sim 0.2.0__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.
- chassis_sim/__init__.py +12 -0
- chassis_sim/bridges/__init__.py +34 -0
- chassis_sim/bridges/adapters.py +200 -0
- chassis_sim/bridges/base.py +217 -0
- chassis_sim/bridges/raycast.py +124 -0
- chassis_sim/bridges/simple_bridge.py +1041 -0
- chassis_sim/chassis/__init__.py +20 -0
- chassis_sim/chassis/ackermann.py +131 -0
- chassis_sim/chassis/base.py +153 -0
- chassis_sim/chassis/diff_drive.py +66 -0
- chassis_sim/chassis/mecanum.py +69 -0
- chassis_sim/chassis/registry.py +81 -0
- chassis_sim/config/__init__.py +15 -0
- chassis_sim/config/curriculum.yaml +248 -0
- chassis_sim/config/env.yaml +49 -0
- chassis_sim/config/reward.yaml +274 -0
- chassis_sim/controllers/__init__.py +13 -0
- chassis_sim/controllers/_geometry.py +181 -0
- chassis_sim/controllers/base.py +253 -0
- chassis_sim/controllers/dwa.py +625 -0
- chassis_sim/controllers/pure_pursuit.py +156 -0
- chassis_sim/envs/__init__.py +51 -0
- chassis_sim/envs/chassis_env.py +1993 -0
- chassis_sim/envs/config.py +779 -0
- chassis_sim/envs/dynamics.py +203 -0
- chassis_sim/envs/observation.py +584 -0
- chassis_sim/envs/path_extractor.py +261 -0
- chassis_sim/envs/path_utils.py +160 -0
- chassis_sim/envs/privileged.py +215 -0
- chassis_sim/envs/reward.py +738 -0
- chassis_sim/envs/safety_gate.py +142 -0
- chassis_sim/eval/__init__.py +773 -0
- chassis_sim/gym_registration.py +51 -0
- chassis_sim/obs/__init__.py +40 -0
- chassis_sim/obs/spec.py +385 -0
- chassis_sim/planning/__init__.py +102 -0
- chassis_sim/planning/hybrid_astar.py +855 -0
- chassis_sim/planning/map_generator.py +1788 -0
- chassis_sim/planning/rs_curve.py +715 -0
- chassis_sim/py.typed +0 -0
- chassis_sim/render/__init__.py +66 -0
- chassis_sim/render/animation.py +130 -0
- chassis_sim/render/figure.py +1082 -0
- chassis_sim/render/recorder.py +232 -0
- chassis_sim/render/scene.py +330 -0
- chassis_sim/scenarios/__init__.py +114 -0
- chassis_sim/scenarios/data/circle.yaml +40 -0
- chassis_sim/scenarios/data/corridor.yaml +28 -0
- chassis_sim/scenarios/data/dense_static.yaml +88 -0
- chassis_sim/scenarios/data/dynamic_crossing.yaml +27 -0
- chassis_sim/scenarios/data/foldback_reverse.yaml +18 -0
- chassis_sim/scenarios/data/maze_grid.yaml +22 -0
- chassis_sim/scenarios/data/obstacles_dense.yaml +17 -0
- chassis_sim/scenarios/data/obstacles_dense_reverse.yaml +17 -0
- chassis_sim/scenarios/data/obstacles_medium.yaml +17 -0
- chassis_sim/scenarios/data/obstacles_sparse.yaml +18 -0
- chassis_sim/scenarios/data/straight.yaml +15 -0
- chassis_sim/scenarios/loader.py +585 -0
- chassis_sim/scenarios/reset_options.py +349 -0
- chassis_sim/scenarios/scenario.py +231 -0
- chassis_sim/sensors/__init__.py +26 -0
- chassis_sim/sensors/specs.py +167 -0
- chassis_sim/sensors/suite.py +393 -0
- chassis_sim-0.2.0.dist-info/METADATA +1236 -0
- chassis_sim-0.2.0.dist-info/RECORD +68 -0
- chassis_sim-0.2.0.dist-info/WHEEL +5 -0
- chassis_sim-0.2.0.dist-info/licenses/LICENSE +21 -0
- chassis_sim-0.2.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,20 @@
|
|
|
1
|
+
"""三种底盘的统一抽象(Ackermann / DiffDrive / Mecanum)。"""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from .ackermann import AckermannChassis
|
|
6
|
+
from .base import Chassis, ChassisCommand
|
|
7
|
+
from .diff_drive import DiffDriveChassis
|
|
8
|
+
from .mecanum import MecanumChassis
|
|
9
|
+
from .registry import CHASSIS_REGISTRY, available_chassis, make_chassis
|
|
10
|
+
|
|
11
|
+
__all__ = [
|
|
12
|
+
"CHASSIS_REGISTRY",
|
|
13
|
+
"AckermannChassis",
|
|
14
|
+
"Chassis",
|
|
15
|
+
"ChassisCommand",
|
|
16
|
+
"DiffDriveChassis",
|
|
17
|
+
"MecanumChassis",
|
|
18
|
+
"available_chassis",
|
|
19
|
+
"make_chassis",
|
|
20
|
+
]
|
|
@@ -0,0 +1,131 @@
|
|
|
1
|
+
"""阿克曼底盘:动作 = [转向, 速度](归一化到 [-1, 1],与主仓 env.action_space 一致)。
|
|
2
|
+
|
|
3
|
+
动作映射**逐字照抄**主仓 /home/zhao/drl-sim/envs/ackermann_env.py 的
|
|
4
|
+
`AckermannEnv._step_impl` 第 820~833 行(含 curvature_action 开关与 v 的分段线性映射),
|
|
5
|
+
该映射只写在 `_map_action()` 一处,由 `command()` 与 `step()` 共用。
|
|
6
|
+
|
|
7
|
+
执行器状态:`step()` 把旧桥「转向滞后一拍」做成显式状态(速度当步生效、ω 用上一帧转角,
|
|
8
|
+
末帧转角留给下一步);`command()` 保持当前帧的纯映射,不读也不写这份状态。
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
from __future__ import annotations
|
|
12
|
+
|
|
13
|
+
import numpy as np
|
|
14
|
+
from gymnasium import spaces
|
|
15
|
+
|
|
16
|
+
from .base import Chassis, ChassisCommand
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
class AckermannChassis(Chassis):
|
|
20
|
+
"""曲率/线性两种转向参数化 + 分段线性速度映射。"""
|
|
21
|
+
|
|
22
|
+
name = "ackermann"
|
|
23
|
+
action_dim = 2
|
|
24
|
+
|
|
25
|
+
def __init__(
|
|
26
|
+
self,
|
|
27
|
+
wheelbase: float = 0.145,
|
|
28
|
+
delta_max: float = 0.526,
|
|
29
|
+
v_min: float = -1.0,
|
|
30
|
+
v_max: float = 1.0,
|
|
31
|
+
curvature_action: bool = True,
|
|
32
|
+
kappa_max: float = 4.0,
|
|
33
|
+
) -> None:
|
|
34
|
+
self.wheelbase = float(wheelbase)
|
|
35
|
+
self.delta_max = float(delta_max)
|
|
36
|
+
self.v_min = float(v_min)
|
|
37
|
+
self.v_max = float(v_max)
|
|
38
|
+
self.curvature_action = bool(curvature_action)
|
|
39
|
+
self.kappa_max = float(kappa_max)
|
|
40
|
+
# 动作是归一化量(主仓 ackermann_env.py:269-273 的 env.action_space 逐位同值),
|
|
41
|
+
# 物理范围 [delta_max] / [v_min, v_max] 在下述映射里施加。
|
|
42
|
+
self._space = spaces.Box(
|
|
43
|
+
low=np.array([-1.0, -1.0], dtype=np.float32),
|
|
44
|
+
high=np.array([1.0, 1.0], dtype=np.float32),
|
|
45
|
+
dtype=np.float32,
|
|
46
|
+
)
|
|
47
|
+
# 执行器状态:上一帧转向角(旧桥 numpy_bridge.py:379 的 self._steer),
|
|
48
|
+
# 首帧为 0.0(numpy_bridge.py:228)。只有 step() 读写它,command() 不碰。
|
|
49
|
+
self._prev_delta: float = 0.0
|
|
50
|
+
|
|
51
|
+
def action_space(self) -> spaces.Box:
|
|
52
|
+
return self._space
|
|
53
|
+
|
|
54
|
+
def _map_action(self, action: np.ndarray) -> tuple[float, float]:
|
|
55
|
+
"""原始动作 -> (当前转向角 δ, 纵向速度 v)。command() 与 step() 共用这一份映射。"""
|
|
56
|
+
action = self._clip_action(action)
|
|
57
|
+
|
|
58
|
+
# ── 以下照抄 drl-sim/envs/ackermann_env.py:822-833(逐字,字节一致)──
|
|
59
|
+
# fmt: off
|
|
60
|
+
mapped_action = action.copy()
|
|
61
|
+
# 转向映射:曲率模式 (R ∝ 1/|a|) vs 线性模式 (δ ∝ a)
|
|
62
|
+
if self.curvature_action:
|
|
63
|
+
kappa = action[0] * self.kappa_max
|
|
64
|
+
mapped_action[0] = np.arctan(np.clip(kappa, -self.kappa_max, self.kappa_max) * self.wheelbase)
|
|
65
|
+
else:
|
|
66
|
+
mapped_action[0] = action[0] * self.delta_max
|
|
67
|
+
|
|
68
|
+
if action[1] >= 0:
|
|
69
|
+
mapped_action[1] = action[1] * self.v_max # [0, +1] → [0, v_max]
|
|
70
|
+
else:
|
|
71
|
+
mapped_action[1] = action[1] * (-self.v_min) # [-1, 0] → [v_min, 0]
|
|
72
|
+
# fmt: on
|
|
73
|
+
# ── 照抄结束 ──
|
|
74
|
+
|
|
75
|
+
return float(mapped_action[0]), float(mapped_action[1])
|
|
76
|
+
|
|
77
|
+
def command(self, action: np.ndarray) -> ChassisCommand:
|
|
78
|
+
"""当前动作的稳态命令:ω 用**本帧**转角。纯函数,不改执行器状态。"""
|
|
79
|
+
delta, v = self._map_action(action)
|
|
80
|
+
# 主仓 numpy_bridge.py:383 的横摆角速度;v == 0 时无转向贡献 ⇒ ω = 0
|
|
81
|
+
omega = 0.0 if v == 0.0 else v / self.wheelbase * np.tan(delta)
|
|
82
|
+
return ChassisCommand(v=v, omega=omega, vy=0.0)
|
|
83
|
+
|
|
84
|
+
def physical_action(self, action: np.ndarray) -> np.ndarray:
|
|
85
|
+
"""归一化动作 -> 外部物理模拟器的 [delta, v](float64)。
|
|
86
|
+
|
|
87
|
+
delta 直接取 `_map_action()` 的**本帧指令值**,不走 `arctan(omega*L/v)`
|
|
88
|
+
反解:反解只在 |v| >= V_EPS 时有定义(零速要沿用上一拍,就成了跨步状态),
|
|
89
|
+
且 200 例随机动作里 64 例差 1 ulp。`_map_action()` 在零速下照给指令 δ,
|
|
90
|
+
因此这里是纯函数:同一 action 永远得到逐位相同的向量。
|
|
91
|
+
"""
|
|
92
|
+
delta, v = self._map_action(action)
|
|
93
|
+
return np.array([delta, v], dtype=np.float64)
|
|
94
|
+
|
|
95
|
+
def step(self, action: np.ndarray) -> ChassisCommand:
|
|
96
|
+
"""推进一步执行器状态:速度立即生效,转向滞后一拍(旧桥 :379-390 的语义)。
|
|
97
|
+
|
|
98
|
+
本步 ω 用**上一帧**转角 self._prev_delta,本帧转角只留给下一步;
|
|
99
|
+
返回后 self._prev_delta 才更新为本帧值,故返回值与状态更新次序不可交换。
|
|
100
|
+
"""
|
|
101
|
+
delta, v = self._map_action(action)
|
|
102
|
+
omega = 0.0 if v == 0.0 else v / self.wheelbase * np.tan(self._prev_delta)
|
|
103
|
+
self._prev_delta = delta
|
|
104
|
+
return ChassisCommand(v=v, omega=omega, vy=0.0)
|
|
105
|
+
|
|
106
|
+
def reset_state(self) -> None:
|
|
107
|
+
"""清零转向滞后状态(首帧 prev=0,与 numpy_bridge.py:228 一致)。"""
|
|
108
|
+
self._prev_delta = 0.0
|
|
109
|
+
|
|
110
|
+
def steering_angle(self) -> float:
|
|
111
|
+
"""当前前轮转角(弧度):step() 更新后的 δ,即轮子此刻的真实朝向。"""
|
|
112
|
+
return float(self._prev_delta)
|
|
113
|
+
|
|
114
|
+
def _limits(self) -> dict:
|
|
115
|
+
# |ω| ≤ |v|·tan|δ| / L,在 v_max 与 delta_max 处取到上界(曲率模式下另有 κ_max 上限)
|
|
116
|
+
return {
|
|
117
|
+
"v_min": self.v_min,
|
|
118
|
+
"v_max": self.v_max,
|
|
119
|
+
"omega_max": self.v_max / self.wheelbase * float(np.tan(self.delta_max)),
|
|
120
|
+
"vy_max": 0.0,
|
|
121
|
+
}
|
|
122
|
+
|
|
123
|
+
def _params(self) -> dict:
|
|
124
|
+
return {
|
|
125
|
+
"wheelbase": self.wheelbase,
|
|
126
|
+
"delta_max": self.delta_max,
|
|
127
|
+
"curvature_action": self.curvature_action,
|
|
128
|
+
"kappa_max": self.kappa_max,
|
|
129
|
+
"v_min": self.v_min,
|
|
130
|
+
"v_max": self.v_max,
|
|
131
|
+
}
|
|
@@ -0,0 +1,153 @@
|
|
|
1
|
+
"""底盘抽象基类:命令载体、统一位姿积分核、几何与激光雷达安装描述。
|
|
2
|
+
|
|
3
|
+
设计约束(本仓 A1 验收点):
|
|
4
|
+
1. 位姿积分只有**一份**实现 —— `Chassis.integrate`,子类不得复制;
|
|
5
|
+
2. 每个车型自己暴露 `action_space`(阿克曼/差速 2 维、麦轮 3 维);
|
|
6
|
+
3. 动作入口统一在 `_clip_action` 校验形状并 clip 到 action_space 边界。
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
from abc import ABC, abstractmethod
|
|
12
|
+
from dataclasses import dataclass
|
|
13
|
+
|
|
14
|
+
import numpy as np
|
|
15
|
+
from gymnasium import spaces
|
|
16
|
+
|
|
17
|
+
|
|
18
|
+
@dataclass(frozen=True)
|
|
19
|
+
class ChassisCommand:
|
|
20
|
+
"""一帧底盘命令,语义固定在**车体系**:
|
|
21
|
+
|
|
22
|
+
v 纵向线速度 (m/s)
|
|
23
|
+
omega 横摆角速度 (rad/s)
|
|
24
|
+
vy 横向线速度 (m/s),仅麦轮非零
|
|
25
|
+
"""
|
|
26
|
+
|
|
27
|
+
v: float
|
|
28
|
+
omega: float
|
|
29
|
+
vy: float = 0.0
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
class Chassis(ABC):
|
|
33
|
+
"""三种底盘共用的抽象。"""
|
|
34
|
+
|
|
35
|
+
name: str = "chassis"
|
|
36
|
+
action_dim: int = 0
|
|
37
|
+
|
|
38
|
+
# 几何默认值 = 主仓 config/env.yaml 的 robot 块
|
|
39
|
+
# (shape 0.38×0.2、前 lidar offset [0.145,0,0]、后 lidar offset [-0.155,0,π])。
|
|
40
|
+
length: float = 0.38
|
|
41
|
+
width: float = 0.20
|
|
42
|
+
FRONT_LIDAR: tuple[float, float, float] = (0.145, 0.0, 0.0)
|
|
43
|
+
REAR_LIDAR: tuple[float, float, float] = (-0.155, 0.0, np.pi)
|
|
44
|
+
|
|
45
|
+
@abstractmethod
|
|
46
|
+
def action_space(self) -> spaces.Box:
|
|
47
|
+
"""本车型的原始动作空间(command() 的输入域)。"""
|
|
48
|
+
|
|
49
|
+
@abstractmethod
|
|
50
|
+
def command(self, action: np.ndarray) -> ChassisCommand:
|
|
51
|
+
"""原始动作 -> ChassisCommand。入参越界先 clip,形状不符抛 ValueError。"""
|
|
52
|
+
|
|
53
|
+
def physical_action(self, action: np.ndarray) -> np.ndarray:
|
|
54
|
+
"""归一化 action -> 交给**外部物理模拟器**的物理向量(本车型自己的顺序)。
|
|
55
|
+
|
|
56
|
+
ackermann : [delta(rad), v(m/s)]
|
|
57
|
+
diff_drive : [v(m/s), omega(rad/s)]
|
|
58
|
+
mecanum : [v(m/s), vy(m/s), omega(rad/s)]
|
|
59
|
+
|
|
60
|
+
具体方法(不是 abstractmethod):第三方子类不必实现它。默认实现基于
|
|
61
|
+
`command()`(纯映射,不读也不写执行器状态):v 恒在最前;麦轮(action_dim
|
|
62
|
+
>= 3)的顺序是 v, vy, omega(不是 ChassisCommand 的字段声明顺序)。
|
|
63
|
+
阿克曼覆盖它 —— 它的 delta 不在 ChassisCommand 里,只能由车型自己的
|
|
64
|
+
`_map_action()` 直接给出(反解只对 |v| >= V_EPS 有定义)。
|
|
65
|
+
"""
|
|
66
|
+
cmd = self.command(action)
|
|
67
|
+
if self.action_dim >= 3:
|
|
68
|
+
return np.array(
|
|
69
|
+
[float(cmd.v), float(cmd.vy), float(cmd.omega)], dtype=np.float64
|
|
70
|
+
)
|
|
71
|
+
return np.array([float(cmd.v), float(cmd.omega)], dtype=np.float64)
|
|
72
|
+
|
|
73
|
+
def step(self, action: np.ndarray) -> ChassisCommand:
|
|
74
|
+
"""推进执行器状态一步,返回**本积分步生效**的命令。默认无滞后。"""
|
|
75
|
+
return self.command(action)
|
|
76
|
+
|
|
77
|
+
def reset_state(self) -> None:
|
|
78
|
+
"""清零执行器内部状态(回合/场景重置时调用)。默认无状态。"""
|
|
79
|
+
return
|
|
80
|
+
|
|
81
|
+
def integrate(
|
|
82
|
+
self,
|
|
83
|
+
x: float,
|
|
84
|
+
y: float,
|
|
85
|
+
theta: float,
|
|
86
|
+
cmd: ChassisCommand,
|
|
87
|
+
dt: float,
|
|
88
|
+
) -> tuple[float, float, float]:
|
|
89
|
+
"""统一位姿积分核(Euler,车体系速度旋到世界系)。
|
|
90
|
+
|
|
91
|
+
x += (v·cosθ − vy·sinθ)·dt
|
|
92
|
+
y += (v·sinθ + vy·cosθ)·dt
|
|
93
|
+
θ += ω·dt
|
|
94
|
+
"""
|
|
95
|
+
v = float(cmd.v)
|
|
96
|
+
vy = float(cmd.vy)
|
|
97
|
+
omega = float(cmd.omega)
|
|
98
|
+
cos_t = np.cos(theta)
|
|
99
|
+
sin_t = np.sin(theta)
|
|
100
|
+
return (
|
|
101
|
+
x + (v * cos_t - vy * sin_t) * dt,
|
|
102
|
+
y + (v * sin_t + vy * cos_t) * dt,
|
|
103
|
+
theta + omega * dt,
|
|
104
|
+
)
|
|
105
|
+
|
|
106
|
+
def footprint(self) -> tuple[float, float]:
|
|
107
|
+
"""车体矩形外廓 (长, 宽),单位米。"""
|
|
108
|
+
return (float(self.length), float(self.width))
|
|
109
|
+
|
|
110
|
+
def lidar_mounts(
|
|
111
|
+
self,
|
|
112
|
+
) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
|
|
113
|
+
"""(前, 后) 激光雷达安装位姿 (x, y, 朝向偏移)。"""
|
|
114
|
+
return (self.FRONT_LIDAR, self.REAR_LIDAR)
|
|
115
|
+
|
|
116
|
+
def steering_angle(self) -> float:
|
|
117
|
+
"""当前前轮转角(弧度);转向轮恒与车身对齐的底盘返回 0.0。"""
|
|
118
|
+
return 0.0
|
|
119
|
+
|
|
120
|
+
def describe(self) -> dict:
|
|
121
|
+
"""车型描述(键名是与控制器层的契约,**不许改名**)。"""
|
|
122
|
+
space = self.action_space()
|
|
123
|
+
return {
|
|
124
|
+
"name": self.name,
|
|
125
|
+
"action_dim": int(self.action_dim),
|
|
126
|
+
"action_space": {
|
|
127
|
+
"low": [float(v) for v in space.low],
|
|
128
|
+
"high": [float(v) for v in space.high],
|
|
129
|
+
},
|
|
130
|
+
"footprint": list(self.footprint()),
|
|
131
|
+
"lidar_mounts": [list(m) for m in self.lidar_mounts()],
|
|
132
|
+
"limits": self._limits(),
|
|
133
|
+
"params": self._params(),
|
|
134
|
+
}
|
|
135
|
+
|
|
136
|
+
def _limits(self) -> dict:
|
|
137
|
+
"""物理量上限快照(子类覆盖):v_min / v_max / omega_max / vy_max,不适用者给数值 0.0。"""
|
|
138
|
+
raise NotImplementedError
|
|
139
|
+
|
|
140
|
+
def _params(self) -> dict:
|
|
141
|
+
"""构造参数快照(子类覆盖);契约键不适用时给 None。"""
|
|
142
|
+
return {}
|
|
143
|
+
|
|
144
|
+
def _clip_action(self, action: np.ndarray) -> np.ndarray:
|
|
145
|
+
"""形状校验 + clip 到 action_space 边界。"""
|
|
146
|
+
space = self.action_space()
|
|
147
|
+
arr = np.asarray(action, dtype=np.float64)
|
|
148
|
+
if arr.shape != space.shape:
|
|
149
|
+
raise ValueError(
|
|
150
|
+
f"{self.name}.command(): action shape {arr.shape} 与期望 shape "
|
|
151
|
+
f"{space.shape}(action_dim={self.action_dim})不一致"
|
|
152
|
+
)
|
|
153
|
+
return np.clip(arr, space.low, space.high)
|
|
@@ -0,0 +1,66 @@
|
|
|
1
|
+
"""差速底盘:动作 = [v, ω] 的归一化量([-1,1]^2),物理速度 = 动作 × 各自上限。
|
|
2
|
+
|
|
3
|
+
与阿克曼、麦轮同一条契约(本仓 A1 裁决):**原始动作一律是归一化量**,物理量
|
|
4
|
+
只在 `command()` 内产生。控制器出口 `to_chassis_action()` 的逆映射
|
|
5
|
+
(`a = v/v_max`)因此与本文的正映射逐位互逆 —— 动作空间若暴露物理界
|
|
6
|
+
(`[-v_max, v_max]`),控制器与底盘之间的往返会被打上 1/v_max 的偏差。
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import numpy as np
|
|
12
|
+
from gymnasium import spaces
|
|
13
|
+
|
|
14
|
+
from .base import Chassis, ChassisCommand
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
class DiffDriveChassis(Chassis):
|
|
18
|
+
name = "diff_drive"
|
|
19
|
+
action_dim = 2
|
|
20
|
+
|
|
21
|
+
def __init__(
|
|
22
|
+
self,
|
|
23
|
+
wheelbase: float = 0.145,
|
|
24
|
+
v_max: float = 1.0,
|
|
25
|
+
omega_max: float = 1.0,
|
|
26
|
+
) -> None:
|
|
27
|
+
self.wheelbase = float(wheelbase)
|
|
28
|
+
self.v_max = float(v_max)
|
|
29
|
+
self.v_min = -self.v_max
|
|
30
|
+
self.omega_max = float(omega_max)
|
|
31
|
+
# 动作是归一化量(与阿克曼/麦轮逐位同界),物理范围 [v_min, v_max] /
|
|
32
|
+
# ±omega_max 只在 command() 的映射里施加。
|
|
33
|
+
self._space = spaces.Box(
|
|
34
|
+
low=np.array([-1.0, -1.0], dtype=np.float32),
|
|
35
|
+
high=np.array([1.0, 1.0], dtype=np.float32),
|
|
36
|
+
dtype=np.float32,
|
|
37
|
+
)
|
|
38
|
+
|
|
39
|
+
def action_space(self) -> spaces.Box:
|
|
40
|
+
return self._space
|
|
41
|
+
|
|
42
|
+
def command(self, action: np.ndarray) -> ChassisCommand:
|
|
43
|
+
action = self._clip_action(action)
|
|
44
|
+
a0, a1 = (float(x) for x in action)
|
|
45
|
+
# 纵向分段线性(与阿克曼/麦轮同款):a0 >= 0 → *v_max,a0 < 0 → *(-v_min)
|
|
46
|
+
v = a0 * (self.v_max if a0 >= 0.0 else -self.v_min)
|
|
47
|
+
return ChassisCommand(v=v, omega=a1 * self.omega_max, vy=0.0)
|
|
48
|
+
|
|
49
|
+
def _limits(self) -> dict:
|
|
50
|
+
return {
|
|
51
|
+
"v_min": self.v_min,
|
|
52
|
+
"v_max": self.v_max,
|
|
53
|
+
"omega_max": self.omega_max,
|
|
54
|
+
"vy_max": 0.0,
|
|
55
|
+
}
|
|
56
|
+
|
|
57
|
+
def _params(self) -> dict:
|
|
58
|
+
return {
|
|
59
|
+
"wheelbase": self.wheelbase,
|
|
60
|
+
"v_max": self.v_max,
|
|
61
|
+
"omega_max": self.omega_max,
|
|
62
|
+
# 契约键(键名不改):差速不适用 ⇒ None
|
|
63
|
+
"delta_max": None,
|
|
64
|
+
"curvature_action": None,
|
|
65
|
+
"kappa_max": None,
|
|
66
|
+
}
|
|
@@ -0,0 +1,69 @@
|
|
|
1
|
+
"""麦轮底盘:动作 = [vx, vy, ω] 的归一化量([-1,1]^3),物理速度 = 动作 × 各自上限。"""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import numpy as np
|
|
6
|
+
from gymnasium import spaces
|
|
7
|
+
|
|
8
|
+
from .base import Chassis, ChassisCommand
|
|
9
|
+
|
|
10
|
+
|
|
11
|
+
class MecanumChassis(Chassis):
|
|
12
|
+
"""全向底盘:保留横向速度,可横移 / 45° 斜行。
|
|
13
|
+
|
|
14
|
+
动作语义与阿克曼、差速一致 —— [-1, 1]^3 的归一化量:
|
|
15
|
+
a[0] 映射到 [v_min, v_max](分段线性:a≥0 乘 v_max、a<0 乘 -v_min,默认对称),
|
|
16
|
+
a[1] 映射到 [-vy_max, vy_max],a[2] 映射到 [-omega_max, omega_max]。
|
|
17
|
+
物理量由 command() 产出、由 describe() 报告。
|
|
18
|
+
"""
|
|
19
|
+
|
|
20
|
+
name = "mecanum"
|
|
21
|
+
action_dim = 3
|
|
22
|
+
|
|
23
|
+
def __init__(
|
|
24
|
+
self,
|
|
25
|
+
lx: float = 0.19,
|
|
26
|
+
ly: float = 0.10,
|
|
27
|
+
v_max: float = 1.0,
|
|
28
|
+
vy_max: float = 0.5,
|
|
29
|
+
omega_max: float = 1.0,
|
|
30
|
+
) -> None:
|
|
31
|
+
self.lx = float(lx)
|
|
32
|
+
self.ly = float(ly)
|
|
33
|
+
self.v_max = float(v_max)
|
|
34
|
+
self.v_min = -self.v_max
|
|
35
|
+
self.vy_max = float(vy_max)
|
|
36
|
+
self.omega_max = float(omega_max)
|
|
37
|
+
self._space = spaces.Box(low=-1.0, high=1.0, shape=(3,), dtype=np.float32)
|
|
38
|
+
|
|
39
|
+
def action_space(self) -> spaces.Box:
|
|
40
|
+
return self._space
|
|
41
|
+
|
|
42
|
+
def command(self, action: np.ndarray) -> ChassisCommand:
|
|
43
|
+
action = self._clip_action(action)
|
|
44
|
+
a0, a1, a2 = (float(x) for x in action)
|
|
45
|
+
# 纵向映射与阿克曼同款分段线性;默认 v_min = -v_max ⇒ 对称
|
|
46
|
+
v = a0 * (self.v_max if a0 >= 0.0 else -self.v_min)
|
|
47
|
+
return ChassisCommand(v=v, vy=a1 * self.vy_max, omega=a2 * self.omega_max)
|
|
48
|
+
|
|
49
|
+
def _limits(self) -> dict:
|
|
50
|
+
return {
|
|
51
|
+
"v_min": self.v_min,
|
|
52
|
+
"v_max": self.v_max,
|
|
53
|
+
"omega_max": self.omega_max,
|
|
54
|
+
"vy_max": self.vy_max,
|
|
55
|
+
}
|
|
56
|
+
|
|
57
|
+
def _params(self) -> dict:
|
|
58
|
+
return {
|
|
59
|
+
"lx": self.lx,
|
|
60
|
+
"ly": self.ly,
|
|
61
|
+
"v_max": self.v_max,
|
|
62
|
+
"vy_max": self.vy_max,
|
|
63
|
+
"omega_max": self.omega_max,
|
|
64
|
+
# 契约键(键名不改):麦轮不适用 ⇒ None
|
|
65
|
+
"wheelbase": None,
|
|
66
|
+
"delta_max": None,
|
|
67
|
+
"curvature_action": None,
|
|
68
|
+
"kappa_max": None,
|
|
69
|
+
}
|
|
@@ -0,0 +1,81 @@
|
|
|
1
|
+
"""车型注册表:字符串 name -> Chassis 实现(装饰器注册,运行期可扩展)。
|
|
2
|
+
|
|
3
|
+
第三方底盘**不必改本仓源码**:在自己的模块里写一次装饰器即可入表,
|
|
4
|
+
之后 `available_chassis()` 能看到它、`make_chassis(名字, **kwargs)` 能构造它::
|
|
5
|
+
|
|
6
|
+
from chassis_sim.chassis import Chassis
|
|
7
|
+
from chassis_sim.chassis.registry import register_chassis
|
|
8
|
+
|
|
9
|
+
@register_chassis("my_rover")
|
|
10
|
+
class MyRover(Chassis):
|
|
11
|
+
...
|
|
12
|
+
|
|
13
|
+
注册是**运行期全局状态**(写进 CHASSIS_REGISTRY 这一个字典):同名重复注册抛
|
|
14
|
+
ValueError 而不是静默覆盖——覆盖会让"谁最终生效"取决于 import 顺序。测试若临时
|
|
15
|
+
注册假底盘,请自行保存/恢复 CHASSIS_REGISTRY(见 tests/test_chassis_registry.py
|
|
16
|
+
的 isolated_registry fixture),否则泄漏的条目会污染别的测试。
|
|
17
|
+
"""
|
|
18
|
+
|
|
19
|
+
from __future__ import annotations
|
|
20
|
+
|
|
21
|
+
from collections.abc import Callable
|
|
22
|
+
from typing import Any
|
|
23
|
+
|
|
24
|
+
from .ackermann import AckermannChassis
|
|
25
|
+
from .base import Chassis
|
|
26
|
+
from .diff_drive import DiffDriveChassis
|
|
27
|
+
from .mecanum import MecanumChassis
|
|
28
|
+
|
|
29
|
+
# name -> 底盘类。register_chassis 是唯一写入点,make_chassis 以它为查表源。
|
|
30
|
+
CHASSIS_REGISTRY: dict[str, type[Chassis]] = {}
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
def register_chassis(name: str) -> Callable[[type[Chassis]], type[Chassis]]:
|
|
34
|
+
"""返回把底盘类登记为 name 的类装饰器。
|
|
35
|
+
|
|
36
|
+
Args:
|
|
37
|
+
name: 车型名(非空字符串),即 make_chassis(name) 的查表键。
|
|
38
|
+
|
|
39
|
+
Returns:
|
|
40
|
+
类装饰器。它原样返回被装饰的类(类的身份/名字都不变),因此
|
|
41
|
+
@register_chassis("x") 只是副作用式注册。
|
|
42
|
+
|
|
43
|
+
Raises:
|
|
44
|
+
ValueError: name 为空(或不是字符串),或该名字已被注册(含内置三车型)。
|
|
45
|
+
消息里给出已占用者的模块与类名,便于定位是谁先注册的。
|
|
46
|
+
"""
|
|
47
|
+
if not isinstance(name, str) or not name:
|
|
48
|
+
raise ValueError(f"车型名必须是非空字符串,收到 {name!r}")
|
|
49
|
+
|
|
50
|
+
def _decorator(cls: type[Chassis]) -> type[Chassis]:
|
|
51
|
+
existing = CHASSIS_REGISTRY.get(name)
|
|
52
|
+
if existing is not None:
|
|
53
|
+
raise ValueError(
|
|
54
|
+
f"车型名 {name!r} 已注册为 "
|
|
55
|
+
f"{existing.__module__}.{existing.__qualname__};"
|
|
56
|
+
f"注册表不允许静默覆盖,请换名或先移除旧条目"
|
|
57
|
+
)
|
|
58
|
+
CHASSIS_REGISTRY[name] = cls
|
|
59
|
+
return cls
|
|
60
|
+
|
|
61
|
+
return _decorator
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
def available_chassis() -> list[str]:
|
|
65
|
+
"""已注册车型名(字典序,便于报告与断言)。"""
|
|
66
|
+
return sorted(CHASSIS_REGISTRY)
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
def make_chassis(name: str, **params: Any) -> Chassis:
|
|
70
|
+
"""按名构造底盘;未知车型抛 KeyError 并列出可用车型。"""
|
|
71
|
+
if name not in CHASSIS_REGISTRY:
|
|
72
|
+
raise KeyError(f"未知底盘 {name!r};可用车型: {', '.join(available_chassis())}")
|
|
73
|
+
return CHASSIS_REGISTRY[name](**params)
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
# ── 内置三车型 ─────────────────────────────────────────────────────────────
|
|
77
|
+
# 等价于在三个类各自的定义处写 @register_chassis("...");因为那三个文件不在本次
|
|
78
|
+
# 改动边界内,这里集中登记。顺序无关(available_chassis 自己排序)。
|
|
79
|
+
register_chassis("ackermann")(AckermannChassis)
|
|
80
|
+
register_chassis("diff_drive")(DiffDriveChassis)
|
|
81
|
+
register_chassis("mecanum")(MecanumChassis)
|
|
@@ -0,0 +1,15 @@
|
|
|
1
|
+
"""包内自带的 env 配置 YAML(package data,随 wheel 发布)。
|
|
2
|
+
|
|
3
|
+
本目录只放数据文件:env.yaml / reward.yaml / curriculum.yaml。
|
|
4
|
+
读取逻辑与 dataclass 在 chassis_sim.envs.config;这里只做转发,使
|
|
5
|
+
`from chassis_sim.config import load_default_config` 也成立。
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from chassis_sim.envs.config import (
|
|
9
|
+
EnvConfig,
|
|
10
|
+
RewardConfig,
|
|
11
|
+
from_config_dir,
|
|
12
|
+
load_default_config,
|
|
13
|
+
)
|
|
14
|
+
|
|
15
|
+
__all__ = ["EnvConfig", "RewardConfig", "from_config_dir", "load_default_config"]
|