# Codex_Impedance_control

**Repository Path**: umbrella9919/codex_-impedance_control

## Basic Information

- **Project Name**: Codex_Impedance_control
- **Description**: No description available
- **Primary Language**: Matlab
- **License**: MulanPSL-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-07-28
- **Last Updated**: 2026-08-03

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# KUKA KR 210 笛卡尔导纳控制仿真

本仓库提供一个经过 MATLAB R2025b 验证的 KUKA KR 210 R2700-2
导纳控制 Simulink 模型。项目已经完成历史模型、调试脚本和旧实验数据清理，
当前只维护最终模型、模型构建与运行函数、结果分析、机器人可视化以及必要的
URDF/STL 资产。

最终模型：

`kuka_admittance_control/models/kuka_admittance_final_validated.slx`

## 当前能力

- 使用直径 `1.000 m` 的水平圆作为未受力期望轨迹。
- 使用连续三轴外力，其中 Z 向包含两个 `80 N` 平滑峰值。
- 顶层包含 `外力模式开关`：开关上路为模拟力，下路为 `[0;0;0]` 0 力；
  当前默认保存为模拟力。
- 按标准笛卡尔导纳关系计算受力位移：

```text
Md * delta_x_ddot + Bd * delta_x_dot + Kd * delta_x = F_ext
x_cmd = x_des + delta_x
```

- 在模型内部执行 KUKA 受限逆运动学与实际正运动学。
- 输出新的六关节角度 `q_cmd` 和实际笛卡尔位置 `x_actual`。
- 在 Simscape Multibody 中显示 KUKA 机器人、蓝色实线期望轨迹和红色
  点划线实际轨迹。
- 支持在 Multibody Explorer 中观察机器人与双轨迹，并可按需要手动切换视角。
- 自动生成轨迹、外力、位置误差和关节连续性对比图。
- 自动生成一张 `XYZABC` 验收图，包含期望位姿与实际位姿共 12 条曲线。
- 自动分别运行模拟力和 0 力两种场景，输出各自的报告和数据图。
- 自动检查末端误差、IK 跟踪误差、力峰值、关节速度和关节加速度。

## 模型结构

```mermaid
flowchart LR
    A["五次平滑的期望圆轨迹 x_des"] --> C["标准笛卡尔导纳"]
    B["连续三轴外力 F_ext"] --> C
    C --> D["柔顺位姿命令 x_cmd"]
    D --> E["KUKA 受限 IK"]
    E --> F["新关节角 q_cmd"]
    F --> G["KUKA 实际 FK"]
    G --> H["实际笛卡尔位置 x_actual"]
    F --> I["Simscape Multibody 机器人"]
    A --> J["期望/实际轨迹对比"]
    H --> J
```

模型中的五个主子系统均可打开：

1. `1_未受力期望圆轨迹`
2. `2_连续三轴外力`
3. `3_标准笛卡尔导纳`
4. `4_KUKA受限逆运动学与实际FK`
5. `5_KUKA机器人三维可视化`

导纳方程由原生 Simulink 离散块展开。KUKA IK/FK 源码内嵌在最终 SLX
的 MATLAB Function 块中，因此打开模型后可以继续逐层查看推演过程。

## 最终参数

| 项目 | 当前值 |
| --- | ---: |
| MATLAB 基线 | R2025b |
| 求解器 | `FixedStepDiscrete` |
| 固定步长 | `0.002 s` |
| 仿真时长 | `7.0 s` |
| 期望圆直径 | `1.000 m` |
| 平移虚拟质量 `Md` | `45 kg` |
| 平移虚拟阻尼 `Bd` | `800 N*s/m` |
| 平移虚拟刚度 `Kd` | `3600 N/m` |
| 最大平移偏移限制 | `0.050 m` |
| 末端位置误差验收线 | `0.020 m` |

参数统一定义在：

`kuka_admittance_control/src/+kuka_admittance/finalParameters.m`

## 目录结构

```text
.
├─ docs/
│  └─ 论文学习/                          # 后续研究继续使用的论文与解析
├─ FK/
│  └─ 6dof/
│     ├─ urdf/6dof.urdf                  # KUKA KR 210 R2700-2 URDF
│     └─ meshes/*.STL                    # 七个机器人外观网格
├─ kuka_admittance_control/
│  ├─ models/
│  │  └─ kuka_admittance_final_validated.slx
│  ├─ scripts/
│  │  ├─ build_kuka_admittance_final_validated.m
│  │  ├─ run_kuka_admittance_final_validated.m
│  │  ├─ open_kuka_admittance_final_validated.m
│  │  └─ visualize_kuka_admittance_final_multiview.m
│  ├─ src/+kuka_admittance/
│  │  ├─ defaultParameters.m
│  │  ├─ finalParameters.m
│  │  ├─ loadKukaRobot.m
│  │  └─ poseVector.m
│  ├─ data/                              # 最终报告、图和演示视频
│  └─ startup.m
├─ AGENTS.md                             # 项目执行约定
├─ README.md                             # 项目完整说明
└─ 任务记录.md                           # 进度、验证结果和恢复入口
```

## 文件职责

| 文件 | 作用 |
| --- | --- |
| `startup.m` | 检查 R2025b，解析仓库相对路径并检查 URDF/STL |
| `defaultParameters.m` | 定义公共控制、机器人、导纳和 IK 默认参数 |
| `finalParameters.m` | 定义最终轨迹、外力、导纳参数和验收阈值 |
| `loadKukaRobot.m` | 跨电脑导入 URDF，并处理 `package://` 网格路径 |
| `poseVector.m` | 将关节姿态转换为 `[x y z roll pitch yaw]` |
| `build_*.m` | 从源码重建最终 SLX，写入机器人、控制器和双轨迹 |
| `run_*.m` | 运行仿真、计算指标、保存 MAT 报告并生成数据图 |
| `open_*.m` | 打开最终模型及关键可推演子系统 |
| `visualize_*.m` | 生成机器人与双轨迹的单视图 MP4 和预览图 |

所有保留的 MATLAB 文件均包含中文文件头、输入输出说明或关键算法注释。

## 环境要求

最低版本为 MATLAB R2025b。需要安装：

- MATLAB
- Simulink
- Simscape
- Simscape Multibody
- Robotics System Toolbox

不再维护 R2025a 或更早版本的兼容副本。

## 快速运行

在 MATLAB 中把仓库根目录设为当前文件夹，然后运行：

```matlab
cd("kuka_admittance_control")
project = startup;
run("scripts/run_kuka_admittance_final_validated.m")
```

运行脚本会在模型缺失或缺少 `外力模式开关` 时自动重建模型，然后分别执行
模拟力和 0 力两种场景的完整仿真与验收。

打开模型和全部关键子系统：

```matlab
run("scripts/open_kuka_admittance_final_validated.m")
```

生成独立单视图机器人运动演示：

```matlab
run("scripts/visualize_kuka_admittance_final_multiview.m")
```

在模型仿真后打开 Multibody Explorer，可在同一窗口中观察机器人、蓝色实线
期望轨迹和红色点划线实际轨迹。

## 最终输出

| 文件 | 内容 |
| --- | --- |
| `data/kuka_admittance_final_validated_report.mat` | 最终时序、误差、力和关节指标 |
| `data/kuka_admittance_final_validated_compare.png` | 模拟力场景的轨迹、外力、误差和关节连续性图 |
| `data/kuka_admittance_final_validated_xyzabc.png` | 期望/实际 `XYZABC` 12 条曲线验收图 |
| `data/kuka_admittance_final_validated_zero_force_report.mat` | 0 力场景最终时序、误差、力和关节指标 |
| `data/kuka_admittance_final_validated_zero_force_compare.png` | 0 力场景的轨迹、外力、误差和关节连续性图 |
| `data/kuka_admittance_final_validated_zero_force_xyzabc.png` | 0 力场景期望/实际 `XYZABC` 12 条曲线验收图 |
| `data/kuka_admittance_final_multiview_motion.mp4` | 单视图机器人运动视频 |
| `data/kuka_admittance_final_multiview_preview.png` | 单视图视频预览图 |

运行脚本不再保存整包 `Simulink.SimulationOutput` 调试文件，避免重复积累
无用历史数据。

## 已验证结果

当前最终参数在 MATLAB R2025b、`FixedStepDiscrete`、`0.002 s` 固定步长下
完成 `3501` 个采样点：

- 最大末端位置误差：`0.018396 m`
- 最终末端位置误差：`0.001509 m`
- 最大内嵌 IK 跟踪误差：`0.000000000 m`
- 最大轴向位置误差：`[0.006292, 0.005728, 0.017530] m`
- 最大关节速度：`0.959885 rad/s`
- 最大关节加速度：`1.748033 rad/s^2`
- 自动验收：通过

同一脚本的 0 力场景也已通过验收：

- 最大末端位置误差：`0.000000 m`
- 最终末端位置误差：`0.000000 m`
- 最大外力范数：`0.000000 N`
- 自动验收：通过

## 适用边界

当前模型用于仿真策略验证，不直接控制实物。Simscape Multibody 机构由
`q_cmd` 运动输入驱动，当前尚未建立关节传感反馈、驱动器动力学、环境接触力
和真实控制柜接口形成的物理闭环。

URDF 导入时仍存在 A2 默认 home position 超出关节限位后被 MATLAB 重置的
已知警告。迁移实机前必须重新核对厂家零位、关节方向、限位、工具坐标系、
传感器坐标系和安全约束。

## 跨电脑恢复

1. 克隆仓库并同步 `origin/master`。
2. 阅读 `AGENTS.md`、`README.md` 和 `任务记录.md`。
3. 安装 MATLAB R2025b+ 及所需工具箱。
4. 如需要使用 AI 直接调用 MATLAB 编辑或验证本仓库，请在该电脑的 AI 客户端
   全局配置中自行安装并配置 MATLAB MCP Core Server；本项目不再提交或维护
   项目级 `.codex` MCP 配置。
5. MATLAB MCP Core Server 官方 Git 仓库：
   [https://github.com/matlab/matlab-mcp-core-server](https://github.com/matlab/matlab-mcp-core-server)。
6. 运行最终 `run_*.m` 脚本复验数值结果。
7. 运行 `open_*.m` 或单视图脚本复验机器人与轨迹显示。

项目内部 MATLAB 源码使用相对路径解析资产，不绑定当前电脑盘符。AI/MCP
配置属于个人开发环境，不应提交到远程仓库。

## 清理与恢复说明

历史 ROCR6II 参考工程、早期 KUKA 模型、调试脚本、旧 MAT 输出和缓存已经
从当前主线删除。后续任务需要查阅旧实现时，可从 Git 提交 `eafa244`
恢复，不应重新复制回主线。论文原文及中文解析属于后续研究资产，继续保留在
`docs/论文学习/`。

## 许可证

许可证见 `LICENSE`。KUKA URDF/STL 与论文资料的使用和再分发还应遵守其
各自来源的许可与版权要求。