diff --git a/README.md b/README.md index df1ca8f2..1b76c47d 100644 --- a/README.md +++ b/README.md @@ -124,7 +124,7 @@ world: offset: [0, 0] # the offset of the world on x and y robot: - kinematics: {name: 'diff'} # omni, diff, acker + kinematics: {name: 'diff'} # omni, omni_angular, diff, acker shape: {name: 'circle', radius: 0.2} # radius state: [1, 1, 0] # x, y, theta goal: [9, 9, 0] # x, y, theta @@ -138,7 +138,7 @@ For more examples, see the [usage directory](https://github.com/hanruihua/ir-sim | **Category** | **Features** | | ---------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| **Kinematics** | Differential Drive mobile Robot · Omnidirectional mobile Robot · Ackermann Steering mobile Robot | +| **Kinematics** | Differential Drive mobile Robot · Omnidirectional mobile Robot · Omnidirectional with Angular control · Ackermann Steering mobile Robot | | **Sensors** | 2D LiDAR · FOV Detector | | **Geometries** | Circle · Rectangle · Polygon · LineString · Binary Grid Map | | **Behaviors** | dash (move directly toward goal) · RVO (Reciprocal Velocity Obstacle) · ORCA (Optimal Reciprocal Collision Avoidance) | diff --git a/docs/locale/zh_CN/LC_MESSAGES/usage/configure_keyboard_Mouse_control.po b/docs/locale/zh_CN/LC_MESSAGES/usage/configure_keyboard_Mouse_control.po index be5884b2..5142319e 100644 --- a/docs/locale/zh_CN/LC_MESSAGES/usage/configure_keyboard_Mouse_control.po +++ b/docs/locale/zh_CN/LC_MESSAGES/usage/configure_keyboard_Mouse_control.po @@ -100,48 +100,64 @@ msgid "`a`" msgstr "`a`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Turn Left" -msgstr "左转" +msgid "Turn Left (diff/acker) / Strafe Left (omni/omni_angular)" +msgstr "左转 (diff/acker) / 左平移 (omni/omni_angular)" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`d`" msgstr "`d`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Turn Right" -msgstr "右转" +msgid "Turn Right (diff/acker) / Strafe Right (omni/omni_angular)" +msgstr "右转 (diff/acker) / 右平移 (omni/omni_angular)" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`q`" msgstr "`q`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Decrease Linear Velocity" -msgstr "降低线速度" +msgid "Rotate Left (omni_angular)" +msgstr "左旋转 (omni_angular)" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`e`" msgstr "`e`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Increase Linear Velocity" -msgstr "提升线速度" +msgid "Rotate Right (omni_angular)" +msgstr "右旋转 (omni_angular)" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`z`" msgstr "`z`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Decrease Angular Velocity" -msgstr "降低角速度" +msgid "Decrease Max Angular Velocity" +msgstr "降低最大角速度" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`c`" msgstr "`c`" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 -msgid "Increase Angular Velocity" -msgstr "提升角速度" +msgid "Increase Max Angular Velocity" +msgstr "提升最大角速度" + +#: ../../source/usage/configure_keyboard_Mouse_control.md:74 +msgid "`shift+z`" +msgstr "`shift+z`" + +#: ../../source/usage/configure_keyboard_Mouse_control.md:74 +msgid "Decrease Max Linear Velocity" +msgstr "降低最大线速度" + +#: ../../source/usage/configure_keyboard_Mouse_control.md:74 +msgid "`shift+c`" +msgstr "`shift+c`" + +#: ../../source/usage/configure_keyboard_Mouse_control.md:74 +msgid "Increase Max Linear Velocity" +msgstr "提升最大线速度" #: ../../source/usage/configure_keyboard_Mouse_control.md:74 msgid "`alt+num`" diff --git a/docs/locale/zh_CN/LC_MESSAGES/usage/configure_robots_obstacles.po b/docs/locale/zh_CN/LC_MESSAGES/usage/configure_robots_obstacles.po index b1fe99bd..e7c65e81 100644 --- a/docs/locale/zh_CN/LC_MESSAGES/usage/configure_robots_obstacles.po +++ b/docs/locale/zh_CN/LC_MESSAGES/usage/configure_robots_obstacles.po @@ -58,12 +58,16 @@ msgstr "关键参数说明" #: ../../source/usage/configure_robots_obstacles.md:54 msgid "" "**`kinematics`:** Defines the movement model of the robot. Name options " -"include `'omni'`, `'diff'`, and `'acker'`." -msgstr "**`kinematics`：** 定义机器人的运动学模型，可选 `'omni'`、`'diff'`、`'acker'`。" +"include `'omni'`, `'omni_angular'`, `'diff'`, and `'acker'`." +msgstr "**`kinematics`：** 定义机器人的运动学模型，可选 `'omni'`、`'omni_angular'`、`'diff'`、`'acker'`。" #: ../../source/usage/configure_robots_obstacles.md:55 -msgid "`'omni'`: Omnidirectional wheels allowing movement in all directions." -msgstr "`'omni'`：全向轮，可向任意方向移动。" +msgid "`'omni'`: Omnidirectional wheels allowing movement in all directions (no orientation control)." +msgstr "`'omni'`：全向轮，可向任意方向移动（无朝向控制）。" + +#: ../../source/usage/configure_robots_obstacles.md:56 +msgid "`'omni_angular'`: Omnidirectional with yaw rate control (can translate and rotate independently)." +msgstr "`'omni_angular'`：带偏航角速度控制的全向移动（可独立平移和旋转）。" #: ../../source/usage/configure_robots_obstacles.md:56 msgid "" @@ -100,13 +104,29 @@ msgid "`omni`" msgstr "`omni`" #: ../../source/usage/configure_robots_obstacles.md:33 -msgid "`[vx, vy]` - velocity in x,y" -msgstr "`[vx, vy]` —— x/y 方向速度" +msgid "`[forward, lateral]` - body-frame velocity" +msgstr "`[forward, lateral]` —— 机体坐标系速度" #: ../../source/usage/configure_robots_obstacles.md:33 msgid "Holonomic robots, drones" msgstr "全向机器人、无人机" +#: ../../source/usage/configure_robots_obstacles.md:33 +msgid "✗ No orientation control" +msgstr "✗ 无朝向控制" + +#: ../../source/usage/configure_robots_obstacles.md:33 +msgid "`omni_angular`" +msgstr "`omni_angular`" + +#: ../../source/usage/configure_robots_obstacles.md:33 +msgid "`[forward, lateral, yaw_rate]` - body-frame velocity + yaw rate" +msgstr "`[forward, lateral, yaw_rate]` —— 机体坐标系速度 + 偏航角速度" + +#: ../../source/usage/configure_robots_obstacles.md:33 +msgid "Holonomic robots with heading" +msgstr "带朝向的全向机器人" + #: ../../source/usage/configure_robots_obstacles.md:33 msgid "✓ Yes" msgstr "✓ 可以" diff --git a/docs/locale/zh_CN/LC_MESSAGES/yaml_config/configuration.po b/docs/locale/zh_CN/LC_MESSAGES/yaml_config/configuration.po index c8a96256..1e20d340 100644 --- a/docs/locale/zh_CN/LC_MESSAGES/yaml_config/configuration.po +++ b/docs/locale/zh_CN/LC_MESSAGES/yaml_config/configuration.po @@ -833,8 +833,8 @@ msgid "`kinematics`" #: ../../source/yaml_config/configuration.md:271 -msgid "Kinematic model of the object. Support name: `diff`, `acker`, `omni`" -msgstr "对象的运动学模型。支持的模型有`diff`, `acker`, `omni`。" +msgid "Kinematic model of the object. Support name: `diff`, `acker`, `omni`, `omni_angular`" +msgstr "对象的运动学模型。支持的模型有`diff`、`acker`、`omni`、`omni_angular`。" #: ../../source/yaml_config/configuration.md:271 #, python-brace-format @@ -864,8 +864,8 @@ msgid "`velocity`" msgstr "`velocity`" #: ../../source/yaml_config/configuration.md:271 -msgid "Initial velocity vector." -msgstr "初始速度向量。" +msgid "Initial velocity vector. Length matches the kinematics action dimension (2 for `diff`/`omni`/`acker`, 3 for `omni_angular`)." +msgstr "初始速度向量。长度与运动学动作维度匹配（`diff`/`omni`/`acker` 为 2，`omni_angular` 为 3）。" #: ../../source/yaml_config/configuration.md:271 msgid "`goal`" @@ -942,36 +942,36 @@ msgid "`vel_min`" msgstr "`vel_min`" #: ../../source/yaml_config/configuration.md:271 -msgid "`[-1, -1]`" -msgstr "`[-1, -1]`" +msgid "`[-1] * action_dim`" +msgstr "`[-1] * action_dim`" #: ../../source/yaml_config/configuration.md:271 -msgid "Minimum velocity limits for each control dimension." -msgstr "各控制维度的最小速度限制。" +msgid "Minimum velocity limits for each control dimension. Length matches the kinematics action dimension." +msgstr "各控制维度的最小速度限制。长度与运动学动作维度匹配。" #: ../../source/yaml_config/configuration.md:271 msgid "`vel_max`" msgstr "`vel_max`" #: ../../source/yaml_config/configuration.md:271 -msgid "`[1, 1]`" -msgstr "`[1, 1]`" +msgid "`[1] * action_dim`" +msgstr "`[1] * action_dim`" #: ../../source/yaml_config/configuration.md:271 -msgid "Maximum velocity limits for each control dimension." -msgstr "各控制维度的最大速度限制。" +msgid "Maximum velocity limits for each control dimension. Length matches the kinematics action dimension." +msgstr "各控制维度的最大速度限制。长度与运动学动作维度匹配。" #: ../../source/yaml_config/configuration.md:271 msgid "`acce`" msgstr "`acce`" #: ../../source/yaml_config/configuration.md:271 -msgid "`[inf, inf]`" -msgstr "`[inf, inf]`" +msgid "`[inf] * action_dim`" +msgstr "`[inf] * action_dim`" #: ../../source/yaml_config/configuration.md:271 -msgid "Acceleration limits." -msgstr "加速度限制。" +msgid "Acceleration limits. Length matches the kinematics action dimension." +msgstr "加速度限制。长度与运动学动作维度匹配。" #: ../../source/yaml_config/configuration.md:271 msgid "`angle_range`" @@ -1224,14 +1224,14 @@ msgid "" "theta]`，其中`theta`表示方向角（弧度）。如果提供的状态元素多于所需，则多余部分会被截断；如果少于所需，缺失值用零填充。" #: ../../source/yaml_config/configuration.md:384 -msgid "**`velocity`** (`list` of `float`, default: `[0, 0]`)" -msgstr "**`velocity`** (由`float`组成的列表, 默认值为`[0, 0]`)" +msgid "**`velocity`** (`list` of `float`, default: `[0] * action_dim`)" +msgstr "**`velocity`**（`float` 列表，默认：`[0] * action_dim`）" #: ../../source/yaml_config/configuration.md:385 msgid "" -"Specifies the initial velocity (list) of the object. The format depends " +"Specifies the initial velocity (list) of the object. The length must match the kinematics action dimension. The format depends " "on the kinematics model:" -msgstr "指定对象的初始速度（列表）。格式取决于运动学模型：" +msgstr "指定对象的初始速度（列表）。长度须与运动学动作维度匹配。格式取决于运动学模型：" #: ../../source/yaml_config/configuration.md:387 msgid "**Format by Kinematics:**" @@ -1248,6 +1248,10 @@ msgid "For `'omni'`: `[forward, lateral]`, body-frame velocities (forward and la msgstr "`'omni'`模型参数由`[forward, lateral]`构成，为机体坐标系下的速度（相对于朝向的前进和横向速度）。" #: ../../source/yaml_config/configuration.md:390 +msgid "For `'omni_angular'`: `[forward, lateral, yaw_rate]`, body-frame velocities plus yaw rate." +msgstr "`'omni_angular'`模型参数由`[forward, lateral, yaw_rate]`构成，为机体坐标系下的速度加偏航角速度。" + +#: ../../source/yaml_config/configuration.md:391 msgid "" "For `'acker'`: Typically `[v, phi]`, where `v` is linear velocity and " "`phi` is steering angle." @@ -1318,6 +1322,10 @@ msgstr "显式定义速度向量的维度。若未指定，则自动从运动学 msgid "`2`: For differential drive `[v, omega]` or omnidirectional `[forward, lateral]`" msgstr "`2`：对于差速驱动`[v, omega]`或全向 `[forward, lateral]`" +#: ../../source/yaml_config/configuration.md:430 +msgid "`3`: For omnidirectional with angular control `[forward, lateral, yaw_rate]`" +msgstr "`3`：对于带角速度控制的全向模型 `[forward, lateral, yaw_rate]`" + #: ../../source/yaml_config/configuration.md:430 msgid "Additional dimensions may be used for more complex kinematics" msgstr "额外维度可用于更复杂的运动学模型" @@ -1419,6 +1427,11 @@ msgstr "**`omni`** — 全向移动，由机体坐标系下的前进和横向速 #: ../../source/yaml_config/configuration.md:495 msgid "" +"**`omni_angular`** — Omnidirectional with angular control, controlled by body-frame speeds and yaw rate (`[forward, lateral, yaw_rate]`)" +msgstr "**`omni_angular`** — 带角速度控制的全向移动，由机体坐标系下的速度和偏航角速度控制（`[forward, lateral, yaw_rate]`）" + +#: ../../source/yaml_config/configuration.md:496 +msgid "" "**`acker`** — Ackermann steering, controlled by linear speed and steering" " angle (`[v, phi]`)" msgstr "**`acker`** — 阿克曼转向，由线速度和转向角控制（`[v, phi]`）" @@ -1447,22 +1460,42 @@ msgid "" msgstr "`noise`（bool）：是否给速度命令添加噪声。默认为 `False`。" #: ../../source/yaml_config/configuration.md:504 -#: ../../source/yaml_config/configuration.md:513 #: ../../source/yaml_config/configuration.md:522 msgid "" "`alpha` (list): noise parameters for velocity commands. Default is " "`[0.03, 0, 0, 0.03]`." msgstr "`alpha`（列表）：速度命令的噪声参数。默认值为 `[0.03, 0, 0, 0.03]`。" +#: ../../source/yaml_config/configuration.md:513 +msgid "" +"`alpha` (list): noise parameters for velocity commands `[alpha_forward, alpha_lateral]`. Default is `[0.03, 0.03]`." +msgstr "`alpha`（列表）：速度命令的噪声参数 `[alpha_forward, alpha_lateral]`。默认值为 `[0.03, 0.03]`。" + #: ../../source/yaml_config/configuration.md:511 msgid "" "`'omni'`: Omnidirectional movement, allowing movement in any direction " "without changing orientation. This type of robot is controlled by " -"velocities along the x and y axes. Optional parameters:" -msgstr "`'omni'`：全向运动，允许在任意方向移动而无需改变朝向。此类机器人通过沿 x 和 y 轴的速度控制。可选参数：" +"body-frame velocities `[forward, lateral]`. Orientation (theta) is preserved but not updated. Optional parameters:" +msgstr "`'omni'`：全向运动，允许在任意方向移动而不改变朝向。此类机器人通过机体坐标系速度 `[forward, lateral]` 控制。朝向角（theta）保持不变。可选参数：" #: ../../source/yaml_config/configuration.md:520 msgid "" +"`'omni_angular'`: Omnidirectional movement with angular velocity control. Extends `omni` by adding a yaw rate channel that integrates orientation (theta). This type of robot is controlled by body-frame velocities `[forward, lateral, yaw_rate]`. Optional parameters:" +msgstr "`'omni_angular'`：带角速度控制的全向运动。在 `omni` 基础上增加偏航角速度通道以积分朝向角（theta）。此类机器人通过机体坐标系速度 `[forward, lateral, yaw_rate]` 控制。可选参数：" + +#: ../../source/yaml_config/configuration.md:521 +msgid "" +"`noise` (bool): whether to add noise to the velocity commands. Default is" +" `False`." +msgstr "`noise`（bool）：是否给速度命令添加噪声。默认为 `False`。" + +#: ../../source/yaml_config/configuration.md:522 +msgid "" +"`alpha` (list): noise parameters for velocity commands `[alpha_forward, alpha_lateral, alpha_yaw]`. Default is `[0.03, 0.03, 0.03]`." +msgstr "`alpha`（列表）：速度命令的噪声参数 `[alpha_forward, alpha_lateral, alpha_yaw]`。默认值为 `[0.03, 0.03, 0.03]`。" + +#: ../../source/yaml_config/configuration.md:528 +msgid "" "`'acker'`: Ackermann steering, typical for car-like vehicles requiring a " "turning radius." msgstr "`'acker'`：阿克曼转向，典型用于类车辆，需要转向半径。" @@ -1483,11 +1516,11 @@ msgstr "`angular`：对象通过线速度和角速度控制。" #: ../../source/yaml_config/configuration.md:532 msgid "" -"**`vel_min`** (`list` of `float`, default: `[-1, -1]`) and **`vel_max`** " -"(`list` of `float`, default: `[1, 1]`)" +"**`vel_min`** (`list` of `float`, default: `[-1] * action_dim`) and **`vel_max`** " +"(`list` of `float`, default: `[1] * action_dim`)" -"**`vel_min`**（`float` 列表，默认：`[-1, -1]`）和 **`vel_max`** （`float` " -"列表，默认：`[1, 1]`）" +"**`vel_min`**（`float` 列表，默认：`[-1] * action_dim`）和 **`vel_max`**（`float` " +"列表，默认：`[1] * action_dim`）" #: ../../source/yaml_config/configuration.md:533 msgid "" @@ -1497,8 +1530,8 @@ msgid "" msgstr "设置每个控制维度的最小和最大速度限制（例如线速度和角速度）。这些约束确保对象的运动保持在可行且安全的范围内。" #: ../../source/yaml_config/configuration.md:535 -msgid "**`acce`** (`list` of `float`, default: `[inf, inf]`)" -msgstr "**`acce`**（`float` 列表，默认：`[inf, inf]`）" +msgid "**`acce`** (`list` of `float`, default: `[inf] * action_dim`)" +msgstr "**`acce`**（`float` 列表，默认：`[inf] * action_dim`）" #: ../../source/yaml_config/configuration.md:536 msgid "" @@ -2626,6 +2659,10 @@ msgstr "混合障碍" msgid "Ackermann Vehicle" msgstr "阿克曼车辆" +#: ../../source/yaml_config/configuration.md +msgid "Omni Angular Robot" +msgstr "全向角速度机器人" + #: ../../source/yaml_config/configuration.md msgid "Sensor Integration" msgstr "传感器集成" diff --git a/docs/source/get_started/quick_start.md b/docs/source/get_started/quick_start.md index f8d130dc..324fb5c8 100644 --- a/docs/source/get_started/quick_start.md +++ b/docs/source/get_started/quick_start.md @@ -34,7 +34,7 @@ world: offset: [0, 0] # the offset of the world on x and y robot: - kinematics: {name: 'diff'} # kinematics of the robot, current name should be one of omni, diff, acker. If not set, this object will be static + kinematics: {name: 'diff'} # kinematics of the robot, current name should be one of omni, omni_angular, diff, acker. If not set, this object will be static shape: {name: 'circle', radius: 0.2} # radius for circle shape state: [1, 1, 0] # x, y, theta, 2d position and orientation goal: [9, 9, 0] # x, y, theta, 2d position and orientation diff --git a/docs/source/usage/configure_keyboard_Mouse_control.md b/docs/source/usage/configure_keyboard_Mouse_control.md index 88b3ebe9..862a97ce 100644 --- a/docs/source/usage/configure_keyboard_Mouse_control.md +++ b/docs/source/usage/configure_keyboard_Mouse_control.md @@ -85,12 +85,14 @@ robot: |----------|---------------------------------| | `w` | Forward | | `s` | Backward | -| `a` | Turn Left | -| `d` | Turn Right | -| `q` | Decrease Linear Velocity | -| `e` | Increase Linear Velocity | -| `z` | Decrease Angular Velocity | -| `c` | Increase Angular Velocity | +| `a` | Turn Left (diff/acker) / Strafe Left (omni/omni_angular) | +| `d` | Turn Right (diff/acker) / Strafe Right (omni/omni_angular) | +| `q` | Rotate Left (omni_angular) | +| `e` | Rotate Right (omni_angular) | +| `z` | Decrease Max Angular Velocity | +| `c` | Increase Max Angular Velocity | +| `shift+z`| Decrease Max Linear Velocity | +| `shift+c`| Increase Max Linear Velocity | | `alt+num`| Change Current Control Robot ID | | `r` | Reset the Environment | | `space` | Toggle Pause/Resume Environment | diff --git a/docs/source/usage/configure_robots_obstacles.md b/docs/source/usage/configure_robots_obstacles.md index 57c15126..064e3fc2 100644 --- a/docs/source/usage/configure_robots_obstacles.md +++ b/docs/source/usage/configure_robots_obstacles.md @@ -51,8 +51,9 @@ robot: ### Important Parameters Explained -- **`kinematics`:** Defines the movement model of the robot. Name options include `'omni'`, `'diff'`, and `'acker'`. - - `'omni'`: Omnidirectional wheels allowing movement in all directions. +- **`kinematics`:** Defines the movement model of the robot. Name options include `'omni'`, `'omni_angular'`, `'diff'`, and `'acker'`. + - `'omni'`: Omnidirectional wheels allowing movement in all directions (no orientation control). + - `'omni_angular'`: Omnidirectional with yaw rate control (can translate and rotate independently). - `'diff'`: Differential drive allowing movement forward/backward and rotation. - `'acker'`: Ackermann steering, typical for car-like robots. diff --git a/docs/source/yaml_config/configuration.md b/docs/source/yaml_config/configuration.md index 719b28af..9bfec54c 100644 --- a/docs/source/yaml_config/configuration.md +++ b/docs/source/yaml_config/configuration.md @@ -300,19 +300,19 @@ All `robot` and `obstacle` entities in the simulation are configured as objects | `name` | `str` or `list` of `str` | `None` | Unique identifier for the object. If omitted, auto-assigned as `"_"`. Supports a list when `number > 1`. | | `number` | `int` | `1` | Number of objects to create. | | `distribution` | `dict` | `{name: manual}` | Defines how multiple objects are distributed. Support name: `manual`, `random`, `circle` | -| `kinematics` | `dict` | `None` | Kinematic model of the object. Support name: `diff`, `acker`, `omni` | +| `kinematics` | `dict` | `None` | Kinematic model of the object. Support name: `diff`, `acker`, `omni`, `omni_angular` | | `shape` | `dict` | `{name: circle}` | Shape of the object. Support name: `circle`, `rectangle`, `polygon` , `linestring` | | `state` | `list` of `float` | `[0, 0, 0]` | Initial state vector of the object. | -| `velocity` | `list` of `float` | `[0, 0]` | Initial velocity vector. | +| `velocity` | `list` of `float` | `[0] * action_dim` | Initial velocity vector. Length matches the kinematics action dimension (2 for `diff`/`omni`/`acker`, 3 for `omni_angular`). | | `goal` | `list` of `float` or `list` of `list` of `float` | `None` | Goal state(s) vector. | | `behavior` | `dict` | `None` | Behavior configuration dictating object movement. Support name: `dash`, `rvo` | | `group_behavior` | `dict` | `None` | Group-level behavior for objects in the same group. Support name: `orca` | | `role` | `str` | `"obstacle"` | Role of the object in the simulation. | | `color` | `str` | `'k'` (black) | Visualization color of the object in the simulation. | | `static` | `bool` | `False` | Indicates if the object is static. | -| `vel_min` | `list` of `float` | `[-1, -1]` | Minimum velocity limits for each control dimension. | -| `vel_max` | `list` of `float` | `[1, 1]` | Maximum velocity limits for each control dimension. | -| `acce` | `list` of `float` | `[inf, inf]` | Acceleration limits. | +| `vel_min` | `list` of `float` | `[-1] * action_dim` | Minimum velocity limits for each control dimension. Length matches the kinematics action dimension. | +| `vel_max` | `list` of `float` | `[1] * action_dim` | Maximum velocity limits for each control dimension. Length matches the kinematics action dimension. | +| `acce` | `list` of `float` | `[inf] * action_dim` | Acceleration limits. Length matches the kinematics action dimension. | | `angle_range` | `list` of `float` | `[-pi, pi]` | Range of orientation angles in radians. | | `goal_threshold` | `float` | `0.1` | Threshold distance to determine goal arrival. | | `sensors` | `list` of `dict` | `None` | List of sensor configurations attached to the object. Support name: `lidar2d` | @@ -394,8 +394,8 @@ All `robot` and `obstacle` entities in the simulation are configured as objects state: [1.0, 1.0, 0.2] ``` -**`velocity`** (`list` of `float`, default: `[0, 0]`) -: Specifies the initial velocity (list) of the object. The format depends on the kinematics model: +**`velocity`** (`list` of `float`, default: `[0] * action_dim`) +: Specifies the initial velocity (list) of the object. The length must match the kinematics action dimension. The format depends on the kinematics model: **Format by Kinematics:** - For `'diff'`: `[v, omega]`, where `v` is linear velocity and `omega` is angular velocity. @@ -522,15 +522,24 @@ All `robot` and `obstacle` entities in the simulation are configured as objects kinematics: {name: 'diff', noise: True, alpha: [0.03, 0, 0, 0.03]} ``` - - `'omni'`: Omnidirectional movement, allowing movement in any direction without changing orientation. This type of robot is controlled by velocities along the x and y axes. Optional parameters: + - `'omni'`: Omnidirectional movement, allowing movement in any direction without changing orientation. This type of robot is controlled by body-frame velocities `[forward, lateral]`. Orientation (theta) is preserved but not updated. Optional parameters: - `noise` (bool): whether to add noise to the velocity commands. Default is `False`. - - `alpha` (list): noise parameters for velocity commands. Default is `[0.03, 0, 0, 0.03]`. + - `alpha` (list): noise parameters for velocity commands `[alpha_forward, alpha_lateral]`. Default is `[0.03, 0.03]`. ```yaml # Example usage - kinematics: {name: 'omni', noise: True, alpha: [0.03, 0, 0, 0.03]} + kinematics: {name: 'omni', noise: True, alpha: [0.03, 0.03]} ``` - + + - `'omni_angular'`: Omnidirectional movement with angular velocity control. Extends `omni` by adding a yaw rate channel that integrates orientation (theta). This type of robot is controlled by body-frame velocities `[forward, lateral, yaw_rate]`. Optional parameters: + - `noise` (bool): whether to add noise to the velocity commands. Default is `False`. + - `alpha` (list): noise parameters for velocity commands `[alpha_forward, alpha_lateral, alpha_yaw]`. Default is `[0.03, 0.03, 0.03]`. + + ```yaml + # Example usage + kinematics: {name: 'omni_angular', noise: True, alpha: [0.03, 0.03, 0.03]} + ``` + - `'acker'`: Ackermann steering, typical for car-like vehicles requiring a turning radius. - `noise` (bool): whether to add noise to the velocity commands. Default is `False`. - `alpha` (list): noise parameters for velocity commands. Default is `[0.03, 0, 0, 0.03]`. @@ -1177,6 +1186,22 @@ robot: ``` ::: +:::{tab-item} Omni Angular Robot +```yaml +robot: + - kinematics: {name: 'omni_angular'} + shape: {name: 'circle', radius: 0.2} + state: [2, 2, 0] + goal: [8, 8, 0] + velocity: [0, 0, 0] + vel_min: [-1, -1, -1] + vel_max: [1, 1, 1] + behavior: {name: 'dash'} + plot: + show_trajectory: True +``` +::: + :::{tab-item} Sensor Integration ```yaml robot: diff --git a/irsim/env/env_base.py b/irsim/env/env_base.py index 93e5e26c..ed6afecd 100644 --- a/irsim/env/env_base.py +++ b/irsim/env/env_base.py @@ -368,10 +368,40 @@ def _objects_check_status(self) -> None: def _assign_keyboard_action(self, action: list[Any]) -> list[Any]: """ Assign the keyboard action to the action list. + + The keyboard produces a ``[linear, lateral, angular]`` (3x1) vector. + For kinematics that expect a different layout the velocity is + converted here so that the keyboard works regardless of robot type. """ - if self._world_param.control_mode == "keyboard" and self.key_id < len(action): - action[self.key_id] = self.key_vel + if ( + self._world_param.control_mode == "keyboard" + and self.key_id < len(action) + and self.key_id < len(self.robot_list) + ): + key_vel = self.key_vel + robot = self.robot_list[self.key_id] + kf_name = robot.kf.name if robot.kf else None + fwd = float(key_vel[0, 0]) # W/S: forward/backward + axis2 = float(key_vel[1, 0]) # A/D: angular (diff/acker) or lateral (omni) + rot = float(key_vel[2, 0]) # Q/E: yaw rate (omni_angular only) + + if kf_name in ("omni", "omni_angular"): + # A/D controls lateral strafe (linear velocity, not angular). + # Rescale from key_ang_max to key_lv_max so that the lateral + # speed matches the forward speed range. + ang_max = self.keyboard.key_ang_max + if ang_max != 0: + axis2 = axis2 / ang_max * self.keyboard.key_lv_max + + if kf_name == "omni": + key_vel = np.array([[fwd], [axis2]]) + elif kf_name == "omni_angular": + key_vel = np.array([[fwd], [axis2], [rot]]) + else: + key_vel = np.array([[fwd], [axis2]]) + + action[self.key_id] = key_vel return action @@ -1358,7 +1388,7 @@ def key_vel(self) -> Any: """Get current keyboard velocity command. Returns: - Any: A 2x1 vector ``[[linear], [angular]]`` from keyboard input. + Any: A 3x1 vector ``[[linear], [lateral], [angular]]`` from keyboard input. """ return self.keyboard.key_vel diff --git a/irsim/gui/keyboard_control.py b/irsim/gui/keyboard_control.py index 06ed5d8e..b12aa318 100644 --- a/irsim/gui/keyboard_control.py +++ b/irsim/gui/keyboard_control.py @@ -43,9 +43,10 @@ class KeyboardControl: Key mappings (both backends): - w/s: Increase/decrease linear velocity (forward/backward) - - a/d: Increase/decrease angular velocity (turn left/right) - - q/e: Decrease/increase maximum linear velocity (key_lv_max) + - a/d: Turn left/right (diff/acker) or strafe left/right (omni/omni_angular) + - q/e: Rotate left/right (omni_angular only) - z/c: Decrease/increase maximum angular velocity (key_ang_max) + - shift+z/c: Decrease/increase maximum linear velocity (key_lv_max) - alt + number: Change current controlled robot id - r: Reset the environment (if ``env_ref`` provided) - space: Toggle pause/resume @@ -80,10 +81,12 @@ def __init__(self, env_ref: Any | None = None, **keyboard_kwargs: Any) -> None: Keyboard mappings (both backends): - w: Move forward - s: Move backward - - a: Turn left - - d: Turn right - - q/e: Decrease/Increase linear velocity - - z/c: Decrease/Increase angular velocity + - a: Turn left (diff/acker) or strafe left (omni/omni_angular) + - d: Turn right (diff/acker) or strafe right (omni/omni_angular) + - q: Rotate left (omni_angular) + - e: Rotate right (omni_angular) + - z/c: Decrease/Increase max angular velocity + - shift+z/c: Decrease/Increase max linear velocity - alt + number: Change current control robot id - r: Reset the environment (if ``env_ref`` provided) - space: Pause/Resume the environment @@ -101,6 +104,7 @@ def __init__(self, env_ref: Any | None = None, **keyboard_kwargs: Any) -> None: self.key_ang_max = keyboard_kwargs.get("key_ang_max", 1.0) self.key_lv = keyboard_kwargs.get("key_lv", 0.0) self.key_ang = keyboard_kwargs.get("key_ang", 0.0) + self.key_rot = keyboard_kwargs.get("key_rot", 0.0) self.key_id = keyboard_kwargs.get("key_id", 0) self.alt_flag = 0 @@ -129,7 +133,7 @@ def __init__(self, env_ref: Any | None = None, **keyboard_kwargs: Any) -> None: if "L" in plt.rcParams["keymap.xscale"]: plt.rcParams["keymap.xscale"].remove("L") - self.key_vel = np.zeros((2, 1)) + self.key_vel = np.zeros((3, 1)) if self._world_param.control_mode == "keyboard": self.logger.info("start to keyboard control") @@ -137,10 +141,12 @@ def __init__(self, env_ref: Any | None = None, **keyboard_kwargs: Any) -> None: commands = [ ["w", "forward"], ["s", "backward"], - ["a", "turn left"], - ["d", "turn right"], - ["q", "decrease linear velocity"], - ["e", "increase linear velocity"], + ["a", "turn left / strafe left"], + ["d", "turn right / strafe right"], + ["q", "rotate left (omni_angular)"], + ["e", "rotate right (omni_angular)"], + ["shift+z", "decrease linear velocity"], + ["shift+c", "increase linear velocity"], ["z", "decrease angular velocity"], ["c", "increase angular velocity"], ["alt+num", "change current control robot id"], @@ -244,8 +250,12 @@ def _on_pynput_press(self, key: Any) -> None: self.key_ang = self.key_ang_max if key.char == "d": self.key_ang = -self.key_ang_max + if key.char == "q": + self.key_rot = self.key_ang_max + if key.char == "e": + self.key_rot = -self.key_ang_max - self.key_vel = np.array([[self.key_lv], [self.key_ang]]) + self.key_vel = np.array([[self.key_lv], [self.key_ang], [self.key_rot]]) except AttributeError: # Handle other special keys that don't have char attribute @@ -273,11 +283,9 @@ def _on_pynput_release(self, key: Any) -> None: if key.char == "d": self.key_ang = 0 if key.char == "q": - self.key_lv_max = self.key_lv_max - 0.2 - self.logger.info(f"current linear velocity: {self.key_lv_max}") + self.key_rot = 0 if key.char == "e": - self.key_lv_max = self.key_lv_max + 0.2 - self.logger.info(f"current linear velocity: {self.key_lv_max}") + self.key_rot = 0 if key.char == "z": self.key_ang_max = self.key_ang_max - 0.2 @@ -285,6 +293,12 @@ def _on_pynput_release(self, key: Any) -> None: if key.char == "c": self.key_ang_max = self.key_ang_max + 0.2 self.logger.info(f"current angular velocity: {self.key_ang_max}") + if key.char == "Z": + self.key_lv_max = self.key_lv_max - 0.2 + self.logger.info(f"current linear velocity: {self.key_lv_max}") + if key.char == "C": + self.key_lv_max = self.key_lv_max + 0.2 + self.logger.info(f"current linear velocity: {self.key_lv_max}") if key.char == "r": self.logger.info("reset the environment") @@ -315,7 +329,7 @@ def _on_pynput_release(self, key: Any) -> None: state = "on" if self.env_ref.display else "off" self.logger.info(f"toggle display: {state}") - self.key_vel = np.array([[self.key_lv], [self.key_ang]]) + self.key_vel = np.array([[self.key_lv], [self.key_ang], [self.key_rot]]) except AttributeError: if "alt" in key.name: @@ -377,8 +391,12 @@ def _on_mpl_press(self, event: Any) -> None: self.key_ang = self.key_ang_max if base == "d": self.key_ang = -self.key_ang_max + if base == "q": + self.key_rot = self.key_ang_max + if base == "e": + self.key_rot = -self.key_ang_max - self.key_vel = np.array([[self.key_lv], [self.key_ang]]) + self.key_vel = np.array([[self.key_lv], [self.key_ang], [self.key_rot]]) def _on_mpl_release(self, event: Any) -> None: """ @@ -388,6 +406,7 @@ def _on_mpl_release(self, event: Any) -> None: event: Matplotlib key release event with ``event.key`` string. """ key = (event.key or "").lower() + has_shift = "shift+" in key base = key.replace("alt+", "").replace("shift+", "").replace("ctrl+", "") if self._world_param.control_mode == "keyboard": @@ -400,15 +419,19 @@ def _on_mpl_release(self, event: Any) -> None: if base == "d": self.key_ang = 0 if base == "q": - self.key_lv_max = self.key_lv_max - 0.2 - self.logger.info(f"current linear velocity: {self.key_lv_max}") + self.key_rot = 0 if base == "e": - self.key_lv_max = self.key_lv_max + 0.2 + self.key_rot = 0 + if base == "z" and has_shift: + self.key_lv_max = self.key_lv_max - 0.2 self.logger.info(f"current linear velocity: {self.key_lv_max}") - if base == "z": + elif base == "z": self.key_ang_max = self.key_ang_max - 0.2 self.logger.info(f"current angular velocity: {self.key_ang_max}") - if base == "c": + if base == "c" and has_shift: + self.key_lv_max = self.key_lv_max + 0.2 + self.logger.info(f"current linear velocity: {self.key_lv_max}") + elif base == "c": self.key_ang_max = self.key_ang_max + 0.2 self.logger.info(f"current angular velocity: {self.key_ang_max}") @@ -462,7 +485,7 @@ def _on_mpl_release(self, event: Any) -> None: if base in ("escape", "esc"): self.env_ref.quit_flag = True - self.key_vel = np.array([[self.key_lv], [self.key_ang]]) + self.key_vel = np.array([[self.key_lv], [self.key_ang], [self.key_rot]]) # Minimal grid table formatter to avoid external dependency def _format_grid_table(self, headers: list[str], rows: list[list[Any]]) -> str: diff --git a/irsim/lib/algorithm/__init__.py b/irsim/lib/algorithm/__init__.py index e7001cec..a50359a5 100644 --- a/irsim/lib/algorithm/__init__.py +++ b/irsim/lib/algorithm/__init__.py @@ -8,13 +8,19 @@ """ from .generation import generate_polygon, random_generate_polygon -from .kinematics import ackermann_kinematics, differential_kinematics, omni_kinematics +from .kinematics import ( + ackermann_kinematics, + differential_kinematics, + omni_angular_kinematics, + omni_kinematics, +) from .rvo import reciprocal_vel_obs __all__ = [ "ackermann_kinematics", "differential_kinematics", "generate_polygon", + "omni_angular_kinematics", "omni_kinematics", "random_generate_polygon", "reciprocal_vel_obs", diff --git a/irsim/lib/algorithm/kinematics.py b/irsim/lib/algorithm/kinematics.py index 113ce56e..1e51bd39 100644 --- a/irsim/lib/algorithm/kinematics.py +++ b/irsim/lib/algorithm/kinematics.py @@ -124,7 +124,7 @@ def ackermann_kinematics( return new_state -@validate_shape(state=2, velocity=2) +@validate_shape(state=3, velocity=2) def omni_kinematics( state: np.ndarray, velocity: np.ndarray, @@ -135,26 +135,97 @@ def omni_kinematics( """ Calculate the next position for an omnidirectional robot. + Uses body-frame velocity: the two components are forward and lateral + speeds relative to the robot heading (theta). Since omni robots have + no yaw control, theta remains unchanged. + Args: - state: A 2x1 vector [x, y] representing the current position. - velocity: A 2x1 vector [vx, vy] representing the current velocities. + state: A 3x1 vector [x, y, theta] representing the current state. + velocity: A 2x1 vector [forward, lateral] in body frame. step_time: The time step for the simulation. noise: Boolean indicating whether to add noise to the velocity (default False). - alpha: List of noise parameters for the velocity model (default [0.03, 0.03]). alpha[0] is for x velocity, alpha[1] is for y velocity. + alpha: List of noise parameters for the velocity model (default [0.03, 0.03]). Returns: - new_position: A 2x1 vector [x, y] representing the next position. + next_state: A 3x1 vector [x, y, theta] representing the next state. + Theta is preserved unchanged. """ if alpha is None: - alpha = [0.03, 0, 0, 0.03] + alpha = [0.03, 0.03] if noise: if len(alpha) < 2: raise ValueError("Parameter 'alpha' must have length >= 2 when noise=True") - std_vx = np.sqrt(alpha[0]) - std_vy = np.sqrt(alpha[-1]) - real_velocity = velocity + rng.normal([[0], [0]], scale=[[std_vx], [std_vy]]) + std_fwd = np.sqrt(alpha[0]) + std_lat = np.sqrt(alpha[1]) + real_velocity = velocity + rng.normal([[0], [0]], scale=[[std_fwd], [std_lat]]) + else: + real_velocity = velocity + + theta = state[2, 0] + cos_t, sin_t = np.cos(theta), np.sin(theta) + fwd = real_velocity[0, 0] + lat = real_velocity[1, 0] + + dx = fwd * cos_t - lat * sin_t + dy = fwd * sin_t + lat * cos_t + + next_state = state[0:3].astype(float) + next_state[0:2] += np.array([[dx], [dy]]) * step_time + + return next_state + + +@validate_shape(state=3, velocity=3) +def omni_angular_kinematics( + state: np.ndarray, + velocity: np.ndarray, + step_time: float, + noise: bool = False, + alpha: list[float] | None = None, +) -> np.ndarray: + """ + Calculate the next state for an omnidirectional robot with angular velocity. + + Uses body-frame velocity: the first two components are forward and lateral + speeds relative to the robot heading, and the third is yaw rate. + + Args: + state: A 3x1 vector [x, y, theta] representing the current position and orientation. + velocity: A 3x1 vector [forward, lateral, yaw_rate] in body frame. + step_time: The time step for the simulation. + noise: Boolean indicating whether to add noise to the velocity (default False). + alpha: List of noise parameters [alpha_fwd, alpha_lat, alpha_yaw] (default [0.03, 0.03, 0.03]). + Each value scales the standard deviation for the corresponding velocity channel. + + Returns: + next_state: A 3x1 vector [x, y, theta] representing the next state. + """ + if alpha is None: + alpha = [0.03, 0.03, 0.03] + + if noise: + if len(alpha) < 3: + raise ValueError("Parameter 'alpha' must have length >= 3 when noise=True") + std_fwd = np.sqrt(alpha[0]) + std_lat = np.sqrt(alpha[1]) + std_yaw = np.sqrt(alpha[2]) + real_velocity = velocity + rng.normal( + [[0], [0], [0]], scale=[[std_fwd], [std_lat], [std_yaw]] + ) else: real_velocity = velocity - return state[0:2] + real_velocity * step_time + theta = state[2, 0] + cos_t, sin_t = np.cos(theta), np.sin(theta) + fwd = real_velocity[0, 0] + lat = real_velocity[1, 0] + + dx = fwd * cos_t - lat * sin_t + dy = fwd * sin_t + lat * cos_t + dtheta = real_velocity[2, 0] + + next_state = state[0:3] + np.array([[dx], [dy], [dtheta]]) * step_time + next_state[2, 0] = WrapToPi(next_state[2, 0]) + + return next_state diff --git a/irsim/lib/behavior/behavior_methods.py b/irsim/lib/behavior/behavior_methods.py index 1e6ed97d..9fe35c14 100644 --- a/irsim/lib/behavior/behavior_methods.py +++ b/irsim/lib/behavior/behavior_methods.py @@ -211,6 +211,38 @@ def beh_omni_rvo( ) +@register_behavior("omni_angular", "dash") +def beh_omni_angular_dash( + ego_object: Any, external_objects: list[Any], **kwargs: Any +) -> np.ndarray: + """ + Behavior function for omnidirectional-angular robot using dash-to-goal behavior. + + Args: + ego_object: The ego robot object. + external_objects (list): List of external objects in the environment. + **kwargs: Additional keyword arguments: + - angle_tolerance (float): Allowable angular deviation, default 0.1. + + Returns: + np.array: Velocity [forward, lateral, yaw_rate] (3x1) in body frame. + """ + + if ego_object.goal is None: + if ego_object._world_param.count % 10 == 0: + ego_object.logger.warning( + "Goal is currently None. This dash behavior is waiting for goal configuration" + ) + return np.zeros((3, 1)) + + state = ego_object.state + goal = ego_object.goal + goal_threshold = ego_object.goal_threshold + _, max_vel = ego_object.get_vel_range() + angle_tolerance = kwargs.get("angle_tolerance", 0.1) + return OmniAngularDash(state, goal, max_vel, goal_threshold, angle_tolerance) + + @register_behavior("acker", "dash") def beh_acker_dash( ego_object: Any, external_objects: list[Any], **kwargs: Any @@ -231,7 +263,7 @@ def beh_acker_dash( if ego_object.goal is None: if ego_object._world_param.count % 10 == 0: ego_object.logger.warning( - "Goal is currently None. This rvo behavior is waiting for goal configuration" + "Goal is currently None. This dash behavior is waiting for goal configuration" ) return np.zeros((2, 1)) @@ -357,27 +389,75 @@ def OmniDash( goal_threshold: float = 0.1, ) -> np.ndarray: """ - Calculate the omnidirectional velocity to reach a goal. + Calculate the body-frame velocity to reach a goal. Args: - state (np.array): Current state [x, y] (2x1). + state (np.array): Current state [x, y, theta] (3x1 or 2x1). goal (np.array): Goal position [x, y] (2x1). - max_vel (np.array): Maximum velocity [vx, vy] (2x1). + max_vel (np.array): Maximum velocity [forward, lateral] (2x1). goal_threshold (float): Distance threshold to consider goal reached (default 0.1). Returns: - np.array: Velocity [vx, vy] (2x1). + np.array: Body-frame velocity [forward, lateral] (2x1). """ distance, radian = relative_position(state, goal) + theta = state[2, 0] if state.shape[0] > 2 else 0.0 if distance > goal_threshold: - vx = max_vel[0, 0] * cos(radian) - vy = max_vel[1, 0] * sin(radian) + diff_angle = WrapToPi(radian - theta) + forward = max_vel[0, 0] * cos(diff_angle) + lateral = max_vel[1, 0] * sin(diff_angle) + else: + forward = 0 + lateral = 0 + + return np.array([[forward], [lateral]]) + + +def OmniAngularDash( + state: np.ndarray, + goal: np.ndarray, + max_vel: np.ndarray, + goal_threshold: float = 0.3, + angle_tolerance: float = 0.1, +) -> np.ndarray: + """ + Calculate body-frame velocity to reach a goal. + + Drives forward and strafes laterally toward the goal while turning + to face it. After arriving at the goal position, rotates in place + to match the goal orientation. + + Args: + state (np.array): Current state [x, y, theta] (3x1). + goal (np.array): Goal state [x, y, theta] (3x1). + max_vel (np.array): Maximum velocity [forward, lateral, yaw_rate] (3x1). + goal_threshold (float): Distance threshold to consider goal reached (default 0.3). + angle_tolerance (float): Allowable angular deviation (default 0.1). + + Returns: + np.array: Body-frame velocity [forward, lateral, yaw_rate] (3x1). + """ + distance, radian = relative_position(state, goal) + theta = state[2, 0] + + if distance > goal_threshold: + diff_angle = WrapToPi(radian - theta) + forward = max_vel[0, 0] * cos(diff_angle) + lateral = max_vel[1, 0] * sin(diff_angle) + target_angle = radian + else: + forward = 0 + lateral = 0 + target_angle = goal[2, 0] if goal.shape[0] > 2 else theta + + diff_angle = WrapToPi(target_angle - theta) + if abs(diff_angle) > angle_tolerance: + yaw_rate = max_vel[2, 0] * np.sign(diff_angle) else: - vx = 0 - vy = 0 + yaw_rate = 0 - return np.array([[vx], [vy]]) + return np.array([[forward], [lateral], [yaw_rate]]) def DiffDash( diff --git a/irsim/lib/handler/kinematics_handler.py b/irsim/lib/handler/kinematics_handler.py index 1ebbde32..25b151bc 100644 --- a/irsim/lib/handler/kinematics_handler.py +++ b/irsim/lib/handler/kinematics_handler.py @@ -7,6 +7,7 @@ from irsim.lib.algorithm.kinematics import ( ackermann_kinematics, differential_kinematics, + omni_angular_kinematics, omni_kinematics, ) @@ -166,7 +167,7 @@ def compute_heading(self, state: np.ndarray, velocity: np.ndarray) -> float: @register_kinematics("omni") class OmniKinematics(KinematicsHandler): action_dim = 2 - min_state_dim = 2 + min_state_dim = 3 state_dim = 3 vel_max: ClassVar[list[float]] = [1, 1] vel_min: ClassVar[list[float]] = [-1, -1] @@ -178,6 +179,8 @@ class OmniKinematics(KinematicsHandler): def __init__(self, name, noise, alpha): super().__init__(name, noise, alpha) + if alpha is None: + self.alpha = [0.03, 0.03] def step( self, state: np.ndarray, velocity: np.ndarray, step_time: float @@ -185,26 +188,90 @@ def step( """Advance omnidirectional state one step. Args: - state (np.ndarray): Current state [x, y, theta, ...]. - velocity (np.ndarray): Velocity [vx, vy]. + state (np.ndarray): Current state [x, y, theta]. + velocity (np.ndarray): Velocity [forward, lateral] in body frame. step_time (float): Time step. Returns: - np.ndarray: New state (x, y updated; rest preserved). + np.ndarray: New state [x, y, theta] (theta preserved). """ - next_position = omni_kinematics( - state[0:2], velocity, step_time, self.noise, self.alpha - ) - return np.concatenate((next_position, state[2:])) + return omni_kinematics(state[0:3], velocity, step_time, self.noise, self.alpha) def velocity_to_xy(self, state: np.ndarray, velocity: np.ndarray) -> np.ndarray: - return velocity + theta = state[2, 0] if state.shape[0] > 2 else 0.0 + cos_t, sin_t = np.cos(theta), np.sin(theta) + fwd = velocity[0, 0] + lat = velocity[1, 0] + vx = fwd * cos_t - lat * sin_t + vy = fwd * sin_t + lat * cos_t + return np.array([[vx], [vy]]) def compute_max_speed(self, vel_max: np.ndarray) -> float: return float(np.linalg.norm(vel_max)) def compute_heading(self, state: np.ndarray, velocity: np.ndarray) -> float: - return float(atan2(velocity[1, 0], velocity[0, 0])) + theta = state[2, 0] if state.shape[0] > 2 else 0.0 + return float(atan2(velocity[1, 0], velocity[0, 0])) + theta + + +@register_kinematics("omni_angular") +class OmniAngularKinematics(KinematicsHandler): + """Omnidirectional kinematics with angular velocity control. + + Velocity is ``[forward, lateral, yaw_rate]`` in body frame. + The kinematics function converts to world-frame internally. + + Note: ``compute_heading`` is intentionally inherited from the base class + (returns ``state[2]``), because this robot has independent yaw control + and its heading IS the orientation angle, unlike :class:`OmniKinematics` + which derives heading from velocity direction. + """ + + action_dim = 3 + min_state_dim = 3 + state_dim = 3 + vel_max: ClassVar[list[float]] = [1, 1, 1] + vel_min: ClassVar[list[float]] = [-1, -1, -1] + acce: ClassVar[list[float]] = [float("inf"), float("inf"), float("inf")] + color = "g" + obstacle_color = "k" + description = None + show_arrow = True + + def __init__(self, name, noise, alpha): + super().__init__(name, noise, alpha) + if alpha is None: + self.alpha = [0.03, 0.03, 0.03] + + def step( + self, state: np.ndarray, velocity: np.ndarray, step_time: float + ) -> np.ndarray: + """Advance omnidirectional-angular state one step. + + Args: + state (np.ndarray): Current state [x, y, theta]. + velocity (np.ndarray): Velocity [forward, lateral, yaw_rate] in body frame. + step_time (float): Time step. + + Returns: + np.ndarray: New state [x, y, theta]. + """ + + return omni_angular_kinematics( + state[0:3], velocity, step_time, self.noise, self.alpha + ) + + def velocity_to_xy(self, state: np.ndarray, velocity: np.ndarray) -> np.ndarray: + theta = state[2, 0] + cos_t, sin_t = np.cos(theta), np.sin(theta) + fwd = velocity[0, 0] + lat = velocity[1, 0] + vx = fwd * cos_t - lat * sin_t + vy = fwd * sin_t + lat * cos_t + return np.array([[vx], [vy]]) + + def compute_max_speed(self, vel_max: np.ndarray) -> float: + return float(np.linalg.norm(vel_max[0:2])) @register_kinematics("diff") diff --git a/tests/test_all_objects.py b/tests/test_all_objects.py index 6370bf53..0c6bc562 100644 --- a/tests/test_all_objects.py +++ b/tests/test_all_objects.py @@ -366,12 +366,11 @@ class XKeyMock: env.keyboard._on_pynput_release(XKeyMock()) assert wp.control_mode == mode0 - # q/e adjust key_lv_max (pynput backend) - prev_lv_max = env.keyboard.key_lv_max - env.keyboard._on_pynput_release(Mock(spec=keyboard.Key, char="e")) - assert env.keyboard.key_lv_max > prev_lv_max + # q/e control rotation velocity (pynput backend) + env.keyboard._on_pynput_press(Mock(spec=keyboard.Key, char="q")) + assert env.keyboard.key_rot == env.keyboard.key_ang_max env.keyboard._on_pynput_release(Mock(spec=keyboard.Key, char="q")) - assert env.keyboard.key_lv_max < prev_lv_max + 0.2 # net change <= 0 + assert env.keyboard.key_rot == 0 # z/c adjust key_ang_max (pynput backend) prev_ang_max = env.keyboard.key_ang_max @@ -599,27 +598,15 @@ def __init__(self, key: str): v = env.robot.velocity.reshape(-1) assert abs(v[1]) <= 1e-9 - # Increase linear max with 'e', press 'w' should reflect new max - prev_lv_max = env.keyboard.key_lv_max - env.keyboard._on_mpl_release(E("e")) - assert env.keyboard.key_lv_max > prev_lv_max - env.keyboard._on_mpl_press(E("w")) - env.step() - v = env.robot.velocity.reshape(-1) - assert pytest.approx(v[0], rel=1e-9, abs=1e-9) == env.keyboard.key_lv_max - env.keyboard._on_mpl_release(E("w")) - env.step() - - # Decrease linear max with 'q', press 'w' should reflect decreased max - prev_lv_max = env.keyboard.key_lv_max + # q/e control rotation velocity (mpl backend) + env.keyboard._on_mpl_press(E("q")) + assert env.keyboard.key_rot == env.keyboard.key_ang_max env.keyboard._on_mpl_release(E("q")) - assert env.keyboard.key_lv_max < prev_lv_max - env.keyboard._on_mpl_press(E("w")) - env.step() - v = env.robot.velocity.reshape(-1) - assert pytest.approx(v[0], rel=1e-9, abs=1e-9) == env.keyboard.key_lv_max - env.keyboard._on_mpl_release(E("w")) - env.step() + assert env.keyboard.key_rot == 0 + env.keyboard._on_mpl_press(E("e")) + assert env.keyboard.key_rot == -env.keyboard.key_ang_max + env.keyboard._on_mpl_release(E("e")) + assert env.keyboard.key_rot == 0 # Increase angular max with 'c', press 'a' should match new max prev_ang_max = env.keyboard.key_ang_max diff --git a/tests/test_behaviors.py b/tests/test_behaviors.py index 290a742b..91f59bc1 100644 --- a/tests/test_behaviors.py +++ b/tests/test_behaviors.py @@ -149,7 +149,7 @@ class TestBehaviorMethodsGoalNone: """Tests for behavior methods when goal is None.""" def test_diff_rvo_goal_none(self, dummy_logger): - """Test diff rvo behavior returns zeros when goal is None (lines 59-63).""" + """Test diff rvo behavior returns zeros when goal is None.""" from irsim.lib.behavior.behavior_methods import beh_diff_rvo _install_dummy_logger() @@ -165,7 +165,7 @@ def test_diff_rvo_goal_none(self, dummy_logger): ego.logger.warning.assert_called_once() def test_omni_dash_goal_none(self, dummy_logger): - """Test omni dash behavior returns zeros when goal is None (lines 129-133).""" + """Test omni dash behavior returns zeros when goal is None.""" from irsim.lib.behavior.behavior_methods import beh_omni_dash _install_dummy_logger() @@ -180,6 +180,44 @@ def test_omni_dash_goal_none(self, dummy_logger): assert np.allclose(result, np.zeros((2, 1))) ego.logger.warning.assert_called_once() + def test_omni_angular_dash_goal_none(self, dummy_logger): + """Test omni_angular dash behavior returns zeros when goal is None.""" + from irsim.lib.behavior.behavior_methods import beh_omni_angular_dash + + _install_dummy_logger() + + ego = Mock() + ego.goal = None + ego._world_param = Mock() + ego._world_param.count = 10 + ego.logger = Mock() + + result = beh_omni_angular_dash(ego, []) + assert np.allclose(result, np.zeros((3, 1))) + ego.logger.warning.assert_called_once() + + def test_omni_angular_dash_with_goal(self, dummy_logger): + """Test omni_angular dash behavior with valid goal.""" + from irsim.lib.behavior.behavior_methods import beh_omni_angular_dash + + _install_dummy_logger() + + ego = Mock() + ego.state = np.array([[0.0], [0.0], [0.0]]) + ego.goal = np.array([[5.0], [3.0], [1.0]]) + ego.goal_threshold = 0.3 + ego.get_vel_range = Mock( + return_value=( + np.array([[-1.0], [-1.0], [-0.5]]), + np.array([[1.0], [1.0], [0.5]]), + ) + ) + ego._world_param = Mock() + ego.logger = Mock() + + result = beh_omni_angular_dash(ego, []) + assert result.shape == (3, 1) + class TestBehaviorMethodsFunctions: """Tests for standalone behavior method functions.""" @@ -203,7 +241,7 @@ def test_diff_rvo_neighbor_none(self): assert result.shape == (2, 1) def test_omni_dash_at_goal(self): - """Test OmniDash when at goal (lines 331-332).""" + """Test OmniDash when at goal.""" from irsim.lib.behavior.behavior_methods import OmniDash state = np.array([[5.0], [5.0], [0.0]]) @@ -213,6 +251,89 @@ def test_omni_dash_at_goal(self): result = OmniDash(state, goal, max_vel, goal_threshold=0.5) assert np.allclose(result, np.zeros((2, 1))) + def test_omni_dash_moving_body_frame(self): + """Test OmniDash produces body-frame velocity toward goal.""" + from irsim.lib.behavior.behavior_methods import OmniDash + + # Robot at origin facing +x, goal ahead + state = np.array([[0.0], [0.0], [0.0]]) + goal = np.array([[5.0], [0.0]]) + max_vel = np.array([[1.0], [1.0]]) + + result = OmniDash(state, goal, max_vel, goal_threshold=0.1) + assert result.shape == (2, 1) + assert result[0, 0] > 0 # forward component positive + + def test_omni_dash_with_heading(self): + """Test OmniDash body-frame velocity with non-zero heading.""" + from irsim.lib.behavior.behavior_methods import OmniDash + + # Robot at origin facing +y (pi/2), goal at (5, 0) = to the right + state = np.array([[0.0], [0.0], [np.pi / 2]]) + goal = np.array([[5.0], [0.0]]) + max_vel = np.array([[1.0], [1.0]]) + + result = OmniDash(state, goal, max_vel, goal_threshold=0.1) + assert result.shape == (2, 1) + # Goal is to the right of heading, so lateral should be negative + assert result[1, 0] < 0 + + def test_omni_angular_dash_moving(self): + """Test OmniAngularDash drives toward goal with yaw.""" + from irsim.lib.behavior.behavior_methods import OmniAngularDash + + state = np.array([[0.0], [0.0], [0.0]]) + goal = np.array([[5.0], [3.0], [1.0]]) + max_vel = np.array([[1.0], [1.0], [0.5]]) + + result = OmniAngularDash(state, goal, max_vel, goal_threshold=0.3) + assert result.shape == (3, 1) + # Should be moving forward and turning toward goal + assert result[0, 0] != 0 or result[1, 0] != 0 + assert result[2, 0] != 0 # yaw rate active + + def test_omni_angular_dash_at_goal_rotate_in_place(self): + """Test OmniAngularDash rotates in place when at goal position.""" + from irsim.lib.behavior.behavior_methods import OmniAngularDash + + state = np.array([[5.0], [5.0], [0.0]]) + goal = np.array([[5.0], [5.0], [1.5]]) # same position, different angle + max_vel = np.array([[1.0], [1.0], [0.5]]) + + result = OmniAngularDash(state, goal, max_vel, goal_threshold=0.5) + assert result.shape == (3, 1) + assert result[0, 0] == 0 # no forward + assert result[1, 0] == 0 # no lateral + assert result[2, 0] > 0 # rotating toward goal angle + + def test_omni_angular_dash_at_goal_aligned(self): + """Test OmniAngularDash stops when at goal and angle aligned.""" + from irsim.lib.behavior.behavior_methods import OmniAngularDash + + state = np.array([[5.0], [5.0], [1.0]]) + goal = np.array([[5.0], [5.0], [1.0]]) # same position and angle + max_vel = np.array([[1.0], [1.0], [0.5]]) + + result = OmniAngularDash( + state, goal, max_vel, goal_threshold=0.5, angle_tolerance=0.1 + ) + assert np.allclose(result, np.zeros((3, 1))) + + def test_omni_angular_dash_goal_no_theta(self): + """Test OmniAngularDash with 2D goal (no theta component).""" + from irsim.lib.behavior.behavior_methods import OmniAngularDash + + state = np.array([[5.0], [5.0], [0.5]]) + goal = np.array([[5.0], [5.0]]) # 2x1 goal, no theta + max_vel = np.array([[1.0], [1.0], [0.5]]) + + result = OmniAngularDash( + state, goal, max_vel, goal_threshold=0.5, angle_tolerance=0.1 + ) + assert result.shape == (3, 1) + # At goal position, goal has no theta so target_angle = theta => no rotation + assert result[2, 0] == 0 + def test_diff_dash_at_goal_and_aligned(self): """Test DiffDash at goal and angle aligned (lines 398, 401).""" from irsim.lib.behavior.behavior_methods import DiffDash diff --git a/tests/test_env.py b/tests/test_env.py index f02cc16e..27a947f2 100644 --- a/tests/test_env.py +++ b/tests/test_env.py @@ -1234,6 +1234,48 @@ def test_desired_omni_vel_no_goal(self, env_factory): assert np.allclose(vel, np.zeros((2, 1))) +class TestAssignKeyboardAction: + """Tests for _assign_keyboard_action with different kinematics.""" + + def test_omni_angular_keyboard_action(self, env_factory): + """Test keyboard action conversion for omni_angular robot.""" + env = env_factory("test_keyboard_control2.yaml") + # Temporarily set control mode to keyboard + original_mode = env._world_param.control_mode + env._world_param.control_mode = "keyboard" + env.keyboard.key_vel = np.array([[1.0], [0.5], [0.3]]) + + # Mock robot kinematics name + robot = env.robot_list[env.key_id] + original_kf_name = robot.kf.name if robot.kf else None + + # Test omni_angular + if robot.kf: + robot.kf.name = "omni_angular" + action = [np.zeros((3, 1)) for _ in range(len(env.robot_list))] + result = env._assign_keyboard_action(action) + vel = result[env.key_id] + assert vel.shape == (3, 1) + assert vel[0, 0] == pytest.approx(1.0) + # axis2 is rescaled from key_ang_max to key_lv_max for omni robots + expected_lat = 0.5 / env.keyboard.key_ang_max * env.keyboard.key_lv_max + assert vel[1, 0] == pytest.approx(expected_lat) + assert vel[2, 0] == pytest.approx(0.3) + + # Test omni + if robot.kf: + robot.kf.name = "omni" + action = [np.zeros((2, 1)) for _ in range(len(env.robot_list))] + result = env._assign_keyboard_action(action) + vel = result[env.key_id] + assert vel.shape == (2, 1) + + # Restore + if robot.kf: + robot.kf.name = original_kf_name + env._world_param.control_mode = original_mode + + class TestMidProcessEdgeCases: """Tests for ObjectBase.mid_process state padding/truncation.""" diff --git a/tests/test_keyboard.py b/tests/test_keyboard.py index 1c1804bb..d969bdca 100644 --- a/tests/test_keyboard.py +++ b/tests/test_keyboard.py @@ -93,15 +93,18 @@ class XKeyMock: env.keyboard._on_pynput_release(XKeyMock()) assert env._world_param.control_mode == mode0 - def test_qe_adjust_lv_max(self, env_factory, mock_keyboard_key): - """Test 'q'/'e' keys adjust linear velocity max.""" + def test_qe_rotate(self, env_factory, mock_keyboard_key): + """Test 'q'/'e' keys control rotation velocity.""" env = env_factory("test_keyboard_control.yaml") env.keyboard._active_only = False # Allow testing without focus - prev_lv_max = env.keyboard.key_lv_max - env.keyboard._on_pynput_release(mock_keyboard_key("e")) - assert env.keyboard.key_lv_max > prev_lv_max + env.keyboard._on_pynput_press(mock_keyboard_key("q")) + assert env.keyboard.key_rot == env.keyboard.key_ang_max env.keyboard._on_pynput_release(mock_keyboard_key("q")) - assert env.keyboard.key_lv_max < prev_lv_max + 0.2 + assert env.keyboard.key_rot == 0 + env.keyboard._on_pynput_press(mock_keyboard_key("e")) + assert env.keyboard.key_rot == -env.keyboard.key_ang_max + env.keyboard._on_pynput_release(mock_keyboard_key("e")) + assert env.keyboard.key_rot == 0 def test_zc_adjust_ang_max(self, env_factory, mock_keyboard_key): """Test 'z'/'c' keys adjust angular velocity max.""" @@ -113,6 +116,16 @@ def test_zc_adjust_ang_max(self, env_factory, mock_keyboard_key): env.keyboard._on_pynput_release(mock_keyboard_key("z")) assert env.keyboard.key_ang_max < prev_ang_max + 0.2 + def test_shift_zc_adjust_lv_max(self, env_factory, mock_keyboard_key): + """Test Shift+Z/C (uppercase) keys adjust linear velocity max.""" + env = env_factory("test_keyboard_control.yaml") + env.keyboard._active_only = False + prev_lv_max = env.keyboard.key_lv_max + env.keyboard._on_pynput_release(mock_keyboard_key("C")) + assert env.keyboard.key_lv_max == pytest.approx(prev_lv_max + 0.2) + env.keyboard._on_pynput_release(mock_keyboard_key("Z")) + assert env.keyboard.key_lv_max == pytest.approx(prev_lv_max) + def test_r_key_sets_reset_flag(self, env_factory, mock_keyboard_key): """Test 'r' key sets reset flag.""" env = env_factory("test_keyboard_control.yaml") @@ -292,24 +305,19 @@ def test_alt_robot_selection(self, env_factory, mock_mpl_event): mpl_kb._on_mpl_press(mock_mpl_event("alt+9")) assert mpl_kb.key_id == 9 - def test_lv_max_adjustment(self, env_factory, mock_mpl_event): - """Test linear velocity max adjustment with e/q keys.""" + def test_qe_rotate_mpl(self, env_factory, mock_mpl_event): + """Test q/e keys control rotation velocity in mpl backend.""" env = env_factory("test_keyboard_control2.yaml") - prev_lv_max = env.keyboard.key_lv_max - env.keyboard._on_mpl_release(mock_mpl_event("e")) - assert env.keyboard.key_lv_max > prev_lv_max - - env.keyboard._on_mpl_press(mock_mpl_event("w")) - env.step() - v = env.robot.velocity.reshape(-1) - assert pytest.approx(v[0], rel=1e-9, abs=1e-9) == env.keyboard.key_lv_max - env.keyboard._on_mpl_release(mock_mpl_event("w")) - env.step() - - prev_lv_max = env.keyboard.key_lv_max + env.keyboard._on_mpl_press(mock_mpl_event("q")) + assert env.keyboard.key_rot == env.keyboard.key_ang_max env.keyboard._on_mpl_release(mock_mpl_event("q")) - assert env.keyboard.key_lv_max < prev_lv_max + assert env.keyboard.key_rot == 0 + + env.keyboard._on_mpl_press(mock_mpl_event("e")) + assert env.keyboard.key_rot == -env.keyboard.key_ang_max + env.keyboard._on_mpl_release(mock_mpl_event("e")) + assert env.keyboard.key_rot == 0 def test_ang_max_adjustment(self, env_factory, mock_mpl_event): """Test angular velocity max adjustment with c/z keys.""" @@ -322,6 +330,17 @@ def test_ang_max_adjustment(self, env_factory, mock_mpl_event): env.keyboard._on_mpl_release(mock_mpl_event("z")) assert env.keyboard.key_ang_max < prev_ang_max + 0.2 + def test_shift_zc_lv_max_adjustment(self, env_factory, mock_mpl_event): + """Test Shift+Z/C keys adjust linear velocity max in mpl backend.""" + env = env_factory("test_keyboard_control2.yaml") + + prev_lv_max = env.keyboard.key_lv_max + env.keyboard._on_mpl_release(mock_mpl_event("shift+c")) + assert env.keyboard.key_lv_max == pytest.approx(prev_lv_max + 0.2) + + env.keyboard._on_mpl_release(mock_mpl_event("shift+z")) + assert env.keyboard.key_lv_max == pytest.approx(prev_lv_max) + def test_space_pause_resume(self, env_factory, mock_mpl_event): """Test space key toggles pause/resume in mpl backend.""" env = env_factory("test_keyboard_control2.yaml") diff --git a/tests/test_kinematics.py b/tests/test_kinematics.py index e7e6d0fc..9e19313d 100644 --- a/tests/test_kinematics.py +++ b/tests/test_kinematics.py @@ -4,6 +4,7 @@ from irsim.lib.algorithm.kinematics import ( ackermann_kinematics, differential_kinematics, + omni_angular_kinematics, omni_kinematics, ) from irsim.lib.handler.kinematics_handler import ( @@ -11,6 +12,7 @@ DifferentialKinematics, KinematicsFactory, KinematicsHandler, + OmniAngularKinematics, OmniKinematics, _kinematics_registry, register_kinematics, @@ -66,21 +68,95 @@ def test_ackermann_kinematics(): def test_omni_kinematics(): - """Test omnidirectional robot kinematics""" - # Test basic movement - state = np.array([[0], [0]]) # x, y - velocity = np.array([[1], [0]]) # vx, vy + """Test omnidirectional robot kinematics with body-frame velocity.""" + # Test forward movement at theta=0 (forward = +x in world) + state = np.array([[0], [0], [0]]) + velocity = np.array([[1], [0]]) # forward, lateral + next_state = omni_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[1], [0], [0]])) + + # Test forward movement at theta=pi/2 (forward = +y in world) + state = np.array([[0], [0], [np.pi / 2]]) + velocity = np.array([[1], [0]]) + next_state = omni_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[0], [1], [np.pi / 2]]), atol=1e-10) + + # Test lateral movement at theta=0 (lateral = +y in world) + state = np.array([[0], [0], [0]]) + velocity = np.array([[0], [1]]) next_state = omni_kinematics(state, velocity, 1.0) - assert np.allclose(next_state, np.array([[1], [0]])) + assert np.allclose(next_state, np.array([[0], [1], [0]]), atol=1e-10) - # Test diagonal movement - velocity = np.array([[1], [1]]) # vx, vy + # Test combined forward+lateral at theta=0 + state = np.array([[0], [0], [0]]) + velocity = np.array([[1], [1]]) next_state = omni_kinematics(state, velocity, 1.0) - assert np.allclose(next_state, np.array([[1], [1]])) + assert np.allclose(next_state, np.array([[1], [1], [0]])) + + # Test theta is preserved + state = np.array([[0], [0], [1.5]]) + velocity = np.array([[1], [0]]) + next_state = omni_kinematics(state, velocity, 1.0) + assert np.allclose(next_state[2, 0], 1.5) # Test with noise next_state_noisy = omni_kinematics(state, velocity, 1.0, noise=True) - assert next_state_noisy.shape == (2, 1) + assert next_state_noisy.shape == (3, 1) + + +def test_omni_angular_kinematics(): + """Test omnidirectional robot kinematics with body-frame velocity.""" + # Test forward movement at theta=0 (forward = +x in world) + state = np.array([[0], [0], [0]]) + velocity = np.array([[1], [0], [0]]) # forward, lateral, yaw_rate + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[1], [0], [0]])) + + # Test forward movement at theta=pi/2 (forward = +y in world) + state = np.array([[0], [0], [np.pi / 2]]) + velocity = np.array([[1], [0], [0]]) + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[0], [1], [np.pi / 2]]), atol=1e-10) + + # Test lateral movement at theta=0 (lateral = -y in world... no, +y) + state = np.array([[0], [0], [0]]) + velocity = np.array([[0], [1], [0]]) # pure lateral + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[0], [1], [0]]), atol=1e-10) + + # Test pure rotation + state = np.array([[0], [0], [0]]) + velocity = np.array([[0], [0], [1]]) + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[0], [0], [1]])) + + # Test combined forward and rotation + state = np.array([[0], [0], [0]]) + velocity = np.array([[1], [0], [0.5]]) + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state, np.array([[1], [0], [0.5]])) + + # Test with noise + next_state_noisy = omni_angular_kinematics( + state, np.array([[1], [1], [0.5]]), 1.0, noise=True + ) + assert next_state_noisy.shape == (3, 1) + + # Test angle wrapping + state = np.array([[0], [0], [np.pi]]) + velocity = np.array([[0], [0], [np.pi]]) + next_state = omni_angular_kinematics(state, velocity, 1.0) + assert np.allclose(next_state[2], 0, atol=1e-10) + + # Test invalid noise parameters + with pytest.raises(ValueError, match="alpha"): + omni_angular_kinematics( + np.array([[0], [0], [0]]), + np.array([[1], [0], [0]]), + 1.0, + noise=True, + alpha=[0.03, 0.03], # Too few parameters + ) def test_kinematics_error_handling(): @@ -115,19 +191,24 @@ class TestKinematicsRegistry: """Tests for the kinematics registry and @register_kinematics decorator.""" def test_builtin_types_registered(self): - """All three built-in kinematics types are in the registry.""" + """All four built-in kinematics types are in the registry.""" assert "diff" in _kinematics_registry assert "omni" in _kinematics_registry + assert "omni_angular" in _kinematics_registry assert "acker" in _kinematics_registry def test_registry_maps_to_correct_classes(self): assert _kinematics_registry["diff"] is DifferentialKinematics assert _kinematics_registry["omni"] is OmniKinematics + assert _kinematics_registry["omni_angular"] is OmniAngularKinematics assert _kinematics_registry["acker"] is AckermannKinematics def test_get_handler_class(self): assert KinematicsFactory.get_handler_class("diff") is DifferentialKinematics assert KinematicsFactory.get_handler_class("omni") is OmniKinematics + assert ( + KinematicsFactory.get_handler_class("omni_angular") is OmniAngularKinematics + ) assert KinematicsFactory.get_handler_class("acker") is AckermannKinematics assert KinematicsFactory.get_handler_class("nonexistent") is None @@ -138,6 +219,9 @@ def test_create_kinematics_uses_registry(self): handler = KinematicsFactory.create_kinematics(name="omni") assert isinstance(handler, OmniKinematics) + handler = KinematicsFactory.create_kinematics(name="omni_angular") + assert isinstance(handler, OmniAngularKinematics) + handler = KinematicsFactory.create_kinematics(name="acker") assert isinstance(handler, AckermannKinematics) @@ -193,7 +277,7 @@ class TestHandlerMetadata: def test_omni_metadata(self): assert OmniKinematics.action_dim == 2 - assert OmniKinematics.min_state_dim == 2 + assert OmniKinematics.min_state_dim == 3 assert OmniKinematics.state_dim == 3 assert OmniKinematics.show_arrow is False @@ -212,6 +296,13 @@ def test_acker_metadata(self): assert AckermannKinematics.description == "car_green.png" assert AckermannKinematics.show_arrow is True + def test_omni_angular_metadata(self): + assert OmniAngularKinematics.action_dim == 3 + assert OmniAngularKinematics.min_state_dim == 3 + assert OmniAngularKinematics.state_dim == 3 + assert OmniAngularKinematics.show_arrow is True + assert OmniAngularKinematics.color == "g" + # --------------------------------------------------------------------------- # Polymorphic method tests @@ -223,10 +314,17 @@ class TestVelocityToXY: def test_omni(self): handler = OmniKinematics("omni", False, None) - state = np.array([[1], [2], [0.5]]) - velocity = np.array([[3], [4]]) + # At theta=0, forward=3 lateral=0 -> world vx=3, vy=0 + state = np.array([[0], [0], [0]]) + velocity = np.array([[3], [0]]) result = handler.velocity_to_xy(state, velocity) - np.testing.assert_array_equal(result, velocity) + np.testing.assert_allclose(result, np.array([[3], [0]]), atol=1e-10) + + # At theta=pi/2, forward=1 lateral=0 -> world vx=0, vy=1 + state = np.array([[0], [0], [np.pi / 2]]) + velocity = np.array([[1], [0]]) + result = handler.velocity_to_xy(state, velocity) + np.testing.assert_allclose(result, np.array([[0], [1]]), atol=1e-10) def test_diff(self): handler = DifferentialKinematics("diff", False, None) @@ -249,6 +347,20 @@ def test_acker(self): result = handler.velocity_to_xy(state, velocity) np.testing.assert_allclose(result, np.array([[2], [0]]), atol=1e-10) + def test_omni_angular(self): + handler = OmniAngularKinematics("omni_angular", False, None) + # At theta=0, forward=3 lateral=0 -> world vx=3, vy=0 + state = np.array([[0], [0], [0]]) + velocity = np.array([[3], [0], [1.0]]) + result = handler.velocity_to_xy(state, velocity) + np.testing.assert_allclose(result, np.array([[3], [0]]), atol=1e-10) + + # At theta=pi/2, forward=1 lateral=0 -> world vx=0, vy=1 + state = np.array([[0], [0], [np.pi / 2]]) + velocity = np.array([[1], [0], [0.5]]) + result = handler.velocity_to_xy(state, velocity) + np.testing.assert_allclose(result, np.array([[0], [1]]), atol=1e-10) + class TestComputeMaxSpeed: """Tests for compute_max_speed on each handler.""" @@ -268,6 +380,12 @@ def test_acker(self): vel_max = np.array([[3], [0.5]]) assert handler.compute_max_speed(vel_max) == pytest.approx(3.0) + def test_omni_angular(self): + handler = OmniAngularKinematics("omni_angular", False, None) + vel_max = np.array([[3], [4], [1]]) + # Only translational components count + assert handler.compute_max_speed(vel_max) == pytest.approx(5.0) + class TestComputeHeading: """Tests for compute_heading on each handler.""" @@ -291,6 +409,13 @@ def test_acker(self): velocity = np.array([[1], [0]]) assert handler.compute_heading(state, velocity) == pytest.approx(2.0) + def test_omni_angular(self): + handler = OmniAngularKinematics("omni_angular", False, None) + state = np.array([[0], [0], [1.5]]) + velocity = np.array([[1], [1], [0.5]]) + # Heading comes from state[2], not velocity direction + assert handler.compute_heading(state, velocity) == pytest.approx(1.5) + # --------------------------------------------------------------------------- # Coverage: edge cases for base class and zero-shape branches diff --git a/usage/02robot_world/robot_omni_angular_world.yaml b/usage/02robot_world/robot_omni_angular_world.yaml new file mode 100644 index 00000000..c9d08108 --- /dev/null +++ b/usage/02robot_world/robot_omni_angular_world.yaml @@ -0,0 +1,29 @@ +world: + height: 10 # the height of the world + width: 10 # the width of the world + step_time: 0.1 # 10Hz calculate each step + sample_time: 0.1 # 10 Hz for render and data extraction + offset: [0, 0] # the offset of the world on x and y + +robot: + - kinematics: {name: 'omni_angular'} # omni, diff, acker + # shape: {name: 'circle', radius: 0.2} # radius + shape: {name: 'rectangle', length: 0.3, width: 1.0} + state: [1, 1, 0] + goal: [9, 9, 0] + # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] + behavior: {name: 'dash'} # move toward to the goal directly + color: 'g' + state_dim: 3 + plot: + show_trajectory: True + # description: 'diff_robot0.png' + + + + + + + + + diff --git a/usage/02robot_world/robot_world.py b/usage/02robot_world/robot_world.py index 71df3f39..d8b431ca 100644 --- a/usage/02robot_world/robot_world.py +++ b/usage/02robot_world/robot_world.py @@ -2,8 +2,10 @@ import irsim -env = irsim.make("robot_world.yaml") +# env = irsim.make("robot_world.yaml") # env = irsim.make("robot_omni_world.yaml") +env = irsim.make("robot_omni_angular_world.yaml") + # env = irsim.make('car_world.yaml') # env = irsim.make("robot_polygon_world.yaml") diff --git a/usage/09keyboard_control/keyboard_control.py b/usage/09keyboard_control/keyboard_control.py index b76f5f6c..cfe8adf2 100644 --- a/usage/09keyboard_control/keyboard_control.py +++ b/usage/09keyboard_control/keyboard_control.py @@ -1,7 +1,8 @@ import irsim # env = irsim.make("keyboard_control_mpl.yaml", save_ani=False, full=False) -env = irsim.make("keyboard_control_pynput.yaml", save_ani=False, full=False) +# env = irsim.make("keyboard_control_pynput.yaml", save_ani=False, full=False) +env = irsim.make("keyboard_control_pynput_omni_angular.yaml", save_ani=False, full=False) for _i in range(1000): env.step() diff --git a/usage/09keyboard_control/keyboard_control_pynput_omni_angular.yaml b/usage/09keyboard_control/keyboard_control_pynput_omni_angular.yaml new file mode 100644 index 00000000..ada54f6f --- /dev/null +++ b/usage/09keyboard_control/keyboard_control_pynput_omni_angular.yaml @@ -0,0 +1,51 @@ +world: + height: 50 # the height of the world + width: 50 # the width of the world + step_time: 0.1 # 10Hz calculate each step + sample_time: 0.1 # 10 Hz for render and data extraction + offset: [0, 0] # the offset of the world on x and y + collision_mode: 'stop' # 'stop', 'unobstructed', + control_mode: 'keyboard' # 'keyboard', 'auto' + plot: + viewpoint: 'robot_0' + +robot: + - kinematics: {name: 'omni_angular'} # omni, diff, acker + shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} + state: [5, 5, 1] + goal: [40, 40, 0] + vel_max: [3.0, 3.0, 3.0] # [forward, lateral, yaw_rate] in body frame + vel_min: [-3.0, -3.0, -3.0] # [forward, lateral, yaw_rate] in body frame + behavior: {name: 'dash'} # move toward to the goal directly + plot: + show_trail: True + traj_color: 'g' + + sensors: + - type: 'lidar2d' + range_min: 0 + range_max: 20 + angle_range: 3.14 + number: 100 + noise: False + std: 1 + angle_std: 0.2 + offset: [0, 0, 0] + alpha: 0.4 + +obstacle: + - number: 1 + distribution: {name: 'manual'} + shape: + - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius + + - number: 1 + distribution: {name: 'manual'} + shape: + - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius + +gui: + keyboard: + key_lv_max: 3.0 + key_ang_max: 1.0 + backend: 'pynput' \ No newline at end of file