Initial project structure for robotic arm feasibility test

- Added project configuration (config.json) with robot model path, environment parameters
- Created 9-DOF robotic arm URDF model (models/manual_robot.urdf)
- Established modular code architecture with empty source files ready for implementation
- Configured Python dependencies (pybullet, PyKDL, urdf_parser_py)
- Added comprehensive project documentation (CLAUDE.md)
- Created .gitignore for Python project

🤖 Generated with [Claude Code](https://claude.ai/code)

Co-Authored-By: Claude <noreply@anthropic.com>

.claude/settings.local.json

@@ -0,0 +1,9 @@
+{
+  "permissions": {
+    "allow": [
+      "Bash(git init:*)"
+    ],
+    "deny": [],
+    "ask": []
+  }
+}

.gitignore

@@ -0,0 +1,63 @@
+# Python
+__pycache__/
+*.py[cod]
+*$py.class
+*.so
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# Virtual environments
+venv/
+env/
+ENV/
+.venv/
+.env/
+
+# IDEs
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# PyCharm
+.idea/
+
+# OS generated files
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.Trashes
+ehthumbs.db
+Thumbs.db
+
+# PyBullet logs
+*.log
+
+# Simulation outputs
+simulation_output/
+results/
+logs/
+
+# Temporary files
+*.tmp
+*.temp

CLAUDE.md

@@ -0,0 +1,114 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## 项目概述
+
+这是一个机械臂运作可行性测试项目，用于测试机械臂从指定基座位置到达指定位置A点，运送物体穿越障碍（一般是墙体上的洞口或其他障碍）到达指定点位B的可行性。
+
+## 技术栈
+
+- **pybullet**: 物理仿真引擎
+- **kdl**: 运动学/动力学库  
+- **AI RRT***: 路径规划算法
+
+## 配置管理原则
+
+**所有配置参数必须从 `config.json` 文件读取，严禁硬编码：**
+- 机械臂模型路径必须从 `config.robot.model_path` 读取
+- 墙体参数从 `config.wall` 读取
+- 洞口参数从 `config.hole` 读取  
+- 任务点A、B从 `config.task_points` 读取
+- 运送物体参数从 `config.transport_object` 读取
+- 仿真参数从 `config.simulation` 读取
+- 修改任何参数只需修改配置文件，无需改动代码
+
+## 项目架构
+
+### 目录结构
+- `config.json`: **核心配置文件**，包含所有运行参数
+- `models/`: 机械臂模型文件目录
+  - `manual_robot.urdf`: 9自由度机械臂URDF配置文件
+- `CLAUDE.md`: 项目开发指南
+
+### 机械臂模型规范
+- 使用URDF格式定义机械臂模型
+- 遵循右手坐标系统：X轴(红色)、Y轴(绿色)、Z轴(蓝色)
+- 关节轴向定义：
+  - `xyz="1 0 0"`: 绕X轴旋转
+  - `xyz="0 1 0"`: 绕Y轴旋转
+  - `xyz="0 0 1"`: 绕Z轴旋转
+
+### 坐标系统
+- 位置定义：`<origin xyz="x y z" rpy="roll pitch yaw"/>`
+- xyz: 位置偏移（米）
+- rpy: 旋转角度（弧度） - roll绕X轴，pitch绕Y轴，yaw绕Z轴
+
+## 开发指导
+
+### 核心功能模块
+1. **机械臂控制模块**: 基于pybullet的仿真控制
+2. **路径规划模块**: 使用RRT*算法进行避障路径规划
+3. **运动学计算模块**: 使用kdl进行正逆运动学计算
+4. **障碍物检测模块**: 碰撞检测和环境感知
+
+## 业务流程（严格按此顺序执行）
+
+### 1. 系统初始化
+1. **读取配置文件**: 从 `config.json` 加载所有配置参数
+2. **验证配置**: 检查所有必要参数是否存在且有效
+3. **初始化pybullet仿真环境**: 使用 `config.simulation` 参数
+
+### 2. 环境构建
+1. **加载机械臂**: 使用 `config.robot.model_path` 指定的URDF文件
+2. **设置机械臂基座**: 使用 `config.robot.base_position` 和 `base_orientation`
+3. **创建墙体障碍**: 根据 `config.wall` 参数创建墙体
+4. **创建洞口**: 在墙体中根据 `config.hole` 参数创建洞口
+5. **放置运送物体**: 在 `config.transport_object.initial_position` 放置物体
+
+### 3. 任务执行
+1. **路径规划阶段1**: 机械臂基座 → A点（取物点）
+2. **抓取物体**: 在A点抓取运送物体
+3. **路径规划阶段2**: A点 → 穿越洞口 → B点（避障路径规划）
+4. **运送物体**: 携带物体穿越障碍到达B点
+5. **释放物体**: 在B点释放物体
+
+### 4. 关键约束
+- **配置驱动**: 所有参数必须从config.json读取
+- **避障要求**: 机械臂和物体不能与墙体碰撞，只能通过洞口
+- **路径平滑**: 使用RRT*算法确保路径可行且平滑
+- **运动学约束**: 遵循机械臂的运动学限制
+
+## 开发规范
+
+### 配置文件使用规范
+```python
+# 正确的配置读取方式
+import json
+
+# 1. 必须首先读取配置文件
+with open('config.json', 'r') as f:
+    config = json.load(f)
+
+# 2. 从配置读取机械臂模型路径
+robot_model_path = config['robot']['model_path']
+
+# 3. 从配置读取所有其他参数
+wall_params = config['wall']
+hole_params = config['hole']
+task_points = config['task_points']
+transport_object = config['transport_object']
+```
+
+### 严禁的做法
+```python
+# ❌ 严禁硬编码路径和参数
+robot_model = "models/manual_robot.urdf"  # 错误！
+wall_position = [2.0, 0.0, 1.0]          # 错误！
+```
+
+### 测试验证
+- 配置文件修改后，程序行为应相应改变
+- 不同的机械臂模型应能正确加载
+- 墙体和洞口参数变化应反映在仿真中
+- A、B点位置调整应影响路径规划结果

Readme.md

@@ -0,0 +1,9 @@
+## 机械臂运作可行性测试
+
+本项目是测试机械臂进行工作的可行性，应用于现实中的工程测试。测试内容是机械臂从指定基座位置到达指定位置A点，运送物体穿越障碍（一般是墙体上的洞口或者其它障碍）到达指定点位B的可行性
+
+### 技术选择
+
+- pybullet
+- kdl
+- AI RRT*

config.json

@@ -0,0 +1,61 @@
+{
+  "robot": {
+    "model_path": "models/manual_robot.urdf",
+    "base_position": [0.0, 0.0, 0.0],
+    "base_orientation": [0.0, 0.0, 0.0]
+  },
+  
+  "wall": {
+    "position": [2.0, 0.0, 1.0],
+    "dimensions": {
+      "width": 3.0,
+      "height": 2.5,
+      "thickness": 0.2
+    },
+    "material": {
+      "color": [0.8, 0.8, 0.8, 1.0],
+      "type": "concrete"
+    }
+  },
+  
+  "hole": {
+    "position_relative_to_wall": [0.0, 0.0, 0.0],
+    "dimensions": {
+      "width": 0.6,
+      "height": 0.6
+    },
+    "shape": "rectangle"
+  },
+  
+  "task_points": {
+    "point_A": {
+      "position": [1.0, 0.5, 0.8],
+      "description": "取物点，与机械臂同侧"
+    },
+    "point_B": {
+      "position": [3.0, 0.5, 0.8],
+      "description": "目标点，墙体另一侧"
+    }
+  },
+  
+  "transport_object": {
+    "initial_position": [1.0, 0.5, 0.5],
+    "dimensions": {
+      "width": 0.1,
+      "height": 0.1,
+      "depth": 0.1
+    },
+    "mass": 0.5,
+    "shape": "cube",
+    "material": {
+      "color": [1.0, 0.0, 0.0, 1.0],
+      "friction": 0.7
+    }
+  },
+  
+  "simulation": {
+    "timestep": 0.01,
+    "gravity": [0.0, 0.0, -9.81],
+    "real_time": false
+  }
+}

models/manual_robot.urdf

@@ -0,0 +1,507 @@
+<!-- 
+=============================================================================
+9-DOF Robot Arm URDF Configuration File
+=============================================================================
+
+URDF Basics:
+1. <link>: Defines rigid body parts (links) of the robot arm
+2. <joint>: Defines connections between two links
+3. Coordinate system: Right-hand coordinate system, X-red, Y-green, Z-blue
+
+Joint axis description:
+- <axis xyz="1 0 0"/>  Rotate around X-axis (red axis)
+- <axis xyz="0 1 0"/>  Rotate around Y-axis (green axis)  
+- <axis xyz="0 0 1"/>  Rotate around Z-axis (blue axis)
+
+Position and rotation:
+- <origin xyz="x y z" rpy="roll pitch yaw"/>
+- xyz: Position offset (meters)
+- rpy: Rotation angles (radians) roll around X-axis, pitch around Y-axis, yaw around Z-axis
+
+Debugging tips:
+1. Modify axis xyz to change joint rotation axis
+2. Modify origin xyz to adjust joint position
+3. Modify origin rpy to adjust link direction
+4. Modify limit to restrict joint motion range
+
+Mass distribution:
+Link masses decrease gradually from base to end effector, following robot arm design principles.
+=============================================================================
+-->
+
+<robot name="custom_robot">
+  
+  <!-- 
+  =============================================================================
+  Material definition - Used to display different colors in simulation
+  =============================================================================
+  -->
+  <material name="orange">
+    <color rgba="1.0 0.5 0.0 1.0"/>  <!-- Orange: red 1.0 green 0.5 blue 0.0 alpha 1.0 -->
+  </material>
+  
+  <material name="blue">
+    <color rgba="0.0 0.5 1.0 1.0"/>  <!-- Blue: red 0.0 green 0.5 blue 1.0 alpha 1.0 -->
+  </material>
+  
+  <!-- 
+  =============================================================================
+  Base link (base_link) - Fixed base of the robot arm
+  =============================================================================
+  This is the root of the robot arm, fixed to the ground and does not move
+  -->
+  <link name="base_link">
+    <!-- Visual appearance -->
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Mesh position and rotation relative to link coordinate system -->
+      <geometry>
+        <mesh filename="meshes/link_0.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+      <material name="orange"/>  <!-- Use orange material -->
+    </visual>
+    
+    <!-- Collision detection -->
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_0.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+    </collision>
+    
+    <!-- Physical properties - using real industrial robot arm parameters -->
+    <!-- Base inertia parameters commented out - KDL kinematics library assumes fixed base, no physical properties needed -->
+    <!--
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>  
+      <mass value="80.0"/>  
+      <inertia ixx="0.64" ixy="0.0" ixz="0.0" iyy="0.64" iyz="0.0" izz="0.72"/>  
+    </inertial>
+    -->
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Joint 1 (joint_1) - Base rotation joint
+  =============================================================================
+  Connects base_link and link_1, usually responsible for horizontal rotation of the robot arm
+  -->
+  <joint name="joint_1" type="revolute">  <!-- revolute=rotation joint -->
+    <parent link="base_link"/>  <!-- Parent link -->
+    <child link="link_1"/>      <!-- Child link -->
+    
+    <!-- Joint position: position relative to parent link (base_link) -->
+    <origin xyz="0 0 0.02" rpy="0 0 0"/>  <!-- Restore original distance: 0.004 / 0.2 = 0.02 meters -->
+    <!-- xyz="0 0 0.02" means joint is 0.02 meters (2 centimeters) above base -->
+    <!-- rpy="0 0 0" means joint has no additional rotation -->
+    
+    <!-- Joint rotation axis -->
+    <axis xyz="0 0 1"/>  
+    <!-- "0 0 1" = rotate around Z-axis (vertical axis) - horizontal rotation -->
+    <!-- If changing to rotate around X-axis, change to "1 0 0" -->
+    <!-- If changing to rotate around Y-axis, change to "0 1 0" -->
+    
+    <!-- Joint limits -->
+    <limit lower="-3.14159" upper="3.14159" effort="500" velocity="2.0"/>  <!-- Base rotation moderate torque, avoid overshoot -->
+    <!-- lower/upper: joint angle limits (radians), -pi to pi means 360 degree rotation -->
+    <!-- effort: maximum torque (N*m) -->
+    <!-- velocity: maximum angular velocity (rad/s) -->
+    
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient: based on diagnostic test results -->
+  </joint>
+  
+  <!-- 
+  =============================================================================
+  Link 1 (link_1) - First active link
+  =============================================================================
+  -->
+  <link name="link_1">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- If mesh direction is wrong, can adjust rpy -->
+      <geometry>
+        <mesh filename="meshes/link_1.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+      <material name="blue"/>  <!-- Use blue material -->
+    </visual>
+    
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_1.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+    </collision>
+    
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="60.0"/>  <!-- Mass (kg) - first joint weight -->
+      <inertia ixx="0.48" ixy="0.0" ixz="0.0" iyy="0.48" iyz="0.0" izz="0.60"/>  <!-- First joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Joint 2 (joint_2) - Shoulder pitch joint
+  =============================================================================
+  Connects link_1 and link_2, usually responsible for up-down swing of robot arm
+  -->
+  <joint name="joint_2" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    
+    <!-- Joint position: position relative to link_1 -->
+    <origin xyz="0 0 0.38" rpy="0 0 0"/>  <!-- Restore original distance: 0.076 / 0.2 = 0.38 meters -->
+    <!-- xyz="0 0 0.38" means joint is 0.38 meters (38 centimeters) above link_1 -->
+    <!-- If joint position is wrong, adjust this xyz value -->
+    
+    <!-- Joint rotation axis -->
+    <axis xyz="1 0 0"/>
+    <!-- "1 0 0" = rotate around X-axis (red axis) - forward-backward swing -->
+    <!-- If motion direction is wrong, can try: -->
+    <!-- "0 1 0" = rotate around Y-axis (green axis) - left-right swing -->
+    <!-- "0 0 1" = rotate around Z-axis (blue axis) - horizontal rotation -->
+    
+    <limit lower="-2.618" upper="2.618" effort="400" velocity="2.0"/>  <!-- Shoulder joint moderate torque, avoid overshoot -->
+    <!-- Modified to -150 degrees to +150 degrees radian value, greatly expand working range -->
+    <!-- Original value: lower="-3.14159" upper="3.14159" approximately +/-180 degrees -->
+    
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <!-- 
+  =============================================================================
+  Link 2 (link_2) - Upper arm link
+  =============================================================================
+  -->
+  <link name="link_2">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <!-- If mesh display direction is wrong, can adjust rpy, for example: -->
+      <!-- rpy="1.57 0 0" rotate 90 degrees around X-axis -->
+      <!-- rpy="0 1.57 0" rotate 90 degrees around Y-axis -->
+      <!-- rpy="0 0 1.57" rotate 90 degrees around Z-axis -->
+      <geometry>
+        <mesh filename="meshes/link_2.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+      <material name="orange"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_2.stl" scale="0.2 0.2 0.2"/>  <!-- Use 0.2x scaled mesh size -->
+      </geometry>
+    </collision>
+    
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="50.0"/>  <!-- Mass (kg) - second joint weight -->
+      <inertia ixx="0.40" ixy="0.0" ixz="0.0" iyy="0.40" iyz="0.0" izz="0.50"/>  <!-- Second joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Joint 3 (joint_3) - Elbow joint
+  =============================================================================
+  Connects link_2 and link_3, usually the elbow bending joint
+  -->
+  <joint name="joint_3" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    
+    <!-- Joint position -->
+    <origin xyz="0 0 0.22" rpy="0 0 0"/>  <!-- Restore original distance: 0.044 / 0.2 = 0.22 meters -->
+    <!-- xyz="0 0 0.22" means joint is 0.22 meters (22 centimeters) above link_2 -->
+    <!-- If need offset, can change to something like xyz="0 0 0.2" -->
+    
+    <!-- Joint rotation axis -->
+    <axis xyz="1 0 0"/>
+    <!-- "1 0 0" = rotate around X-axis -->
+    <!-- Elbow usually rotates around X-axis or Y-axis, adjust according to actual situation -->
+    
+    <limit lower="-2.356" upper="2.356" effort="300" velocity="2.0"/>  <!-- Elbow joint moderate torque, avoid overshoot -->
+    <!-- Modified to -135 degrees to +135 degrees radian value, greatly expand working range -->
+    <!-- Original value: lower="-3.14159" upper="3.14159" approximately +/-180 degrees -->
+    
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <!-- 
+  =============================================================================
+  Link 3 (link_3) - Forearm link
+  =============================================================================
+  Note: This link's mass is set to non-zero to avoid simulation problems
+  -->
+  <link name="link_3">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_3.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="blue"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_3.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="40.0"/>  <!-- Mass (kg) - third joint weight -->
+      <inertia ixx="0.32" ixy="0.0" ixz="0.0" iyy="0.32" iyz="0.0" izz="0.38"/>  <!-- Third joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Joint 4 (joint_4) - Wrist rotation joint 1
+  =============================================================================
+  Connects link_3 and link_4, enters wrist joint area
+  -->
+  <joint name="joint_4" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    
+    <!-- Joint position -->
+    <origin xyz="0 -0.47 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.094 / 0.2 = -0.47 meters -->
+    <!-- xyz="0 -0.47 0" means joint is 0.47 meters (47 centimeters) behind link_3 -->
+    
+    <!-- Joint rotation axis -->
+    <axis xyz="0 0 1"/>
+    <!-- "0 0 1" = rotate around Z-axis (blue axis) - wrist rotation -->
+    <!-- Wrist joints usually have multiple rotation axes, this is the first one -->
+    
+    <limit lower="-3.14159" upper="3.14159" effort="800" velocity="2.0"/>  <!-- Wrist joint moderate torque, fully open to +/-180 degrees -->
+    <!-- Modified to -180 degrees to +180 degrees radian value, maximum working range -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <!-- 
+  =============================================================================
+  Link 4 (link_4) - Wrist link 1
+  =============================================================================
+  -->
+  <link name="link_4">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_4.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="orange"/>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_4.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="25.0"/>  <!-- Mass (kg) - fourth joint weight -->
+      <inertia ixx="0.25" ixy="0.0" ixz="0.0" iyy="0.25" iyz="0.0" izz="0.30"/>  <!-- Fourth joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Joints 5-9 - Wrist and end effector joint group
+  =============================================================================
+  These joints form the wrist and end effector part of the robot arm
+  -->
+  
+  <!-- Joint 5 - Wrist rotation joint 2 -->
+  <joint name="joint_5" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <origin xyz="0 -0.18 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.036 / 0.2 = -0.18 meters -->  <!-- Position offset 0.18 meters (18 centimeters) -->
+    <axis xyz="0 0 1"/>  <!-- Rotate around Z-axis, can change to "1 0 0" or "0 1 0" -->
+    <limit lower="-3.14159" upper="3.14159" effort="800" velocity="2.0"/>  <!-- Wrist joint moderate torque, fully open to +/-180 degrees -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <link name="link_5">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- If need to adjust mesh direction, modify rpy -->
+      <geometry>
+        <mesh filename="meshes/link_5.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_5.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="20.0"/>  <!-- Mass (kg) - fifth joint weight -->
+      <inertia ixx="0.20" ixy="0.0" ixz="0.0" iyy="0.20" iyz="0.0" izz="0.24"/>  <!-- Fifth joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- Joint 6 - Wrist pitch joint -->
+  <joint name="joint_6" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <origin xyz="0 -0.17 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.034 / 0.2 = -0.17 meters -->   <!-- Joint position offset 0.17 meters (17 centimeters) -->
+    <axis xyz="0 0 1"/>  <!-- Rotation axis, adjust as needed -->
+    <limit lower="-3.14159" upper="3.14159" effort="800" velocity="2.0"/>  <!-- Wrist joint moderate torque, fully open to +/-180 degrees -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <link name="link_6">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_6.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_6.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="15.0"/>  <!-- Mass (kg) - sixth joint weight -->
+      <inertia ixx="0.15" ixy="0.0" ixz="0.0" iyy="0.15" iyz="0.0" izz="0.18"/>  <!-- Sixth joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- Joint 7 - Wrist roll joint -->
+  <joint name="joint_7" type="revolute">
+    <parent link="link_6"/>
+    <child link="link_7"/>
+    <origin xyz="0 -0.57 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.114 / 0.2 = -0.57 meters -->  <!-- Position offset 0.57 meters (57 centimeters) -->
+    <axis xyz="0 1 0"/>  <!-- Rotate around Y-axis (green axis) -->
+    <limit lower="-3.14159" upper="3.14159" effort="300" velocity="2.0"/>  <!-- Relax to +/-180 degrees consistent with config file -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <link name="link_7">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_7.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_7.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="10.0"/>  <!-- Mass (kg) - seventh joint weight -->
+      <inertia ixx="0.10" ixy="0.0" ixz="0.0" iyy="0.10" iyz="0.0" izz="0.12"/>  <!-- Seventh joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- Joint 8 - End effector joint 1 -->
+  <joint name="joint_8" type="revolute">
+    <parent link="link_7"/>
+    <child link="link_8"/>
+    <origin xyz="0 -0.11 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.022 / 0.2 = -0.11 meters -->  <!-- Position offset 0.11 meters (11 centimeters) -->
+    <axis xyz="1 0 0"/>  <!-- Rotate around X-axis (red axis) -->
+    <limit lower="-3.14159" upper="3.14159" effort="300" velocity="2.0"/>  <!-- Relax to +/-180 degrees consistent with config file -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <link name="link_8">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_8.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_8.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="7.0"/>  <!-- Mass (kg) - eighth joint weight -->
+      <inertia ixx="0.07" ixy="0.0" ixz="0.0" iyy="0.07" iyz="0.0" izz="0.08"/>  <!-- Eighth joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- Joint 9 - End effector joint 2 -->
+  <joint name="joint_9" type="revolute">
+    <parent link="link_8"/>
+    <child link="link_9"/>
+    <origin xyz="0 -0.39 0" rpy="0 0 0"/>  <!-- Restore original distance: -0.078 / 0.2 = -0.39 meters -->
+    <axis xyz="0 1 0"/>  <!-- Rotate around Y-axis, can adjust as needed -->
+    <limit lower="-3.14159" upper="3.14159" effort="300" velocity="2.0"/>  <!-- Relax to +/-180 degrees consistent with config file -->
+    <dynamics damping="0.0"/>  <!-- Optimal damping coefficient -->
+  </joint>
+  
+  <!-- 
+  =============================================================================
+  Link 9 - End effector
+  =============================================================================
+  This is the last link of the robot arm, usually connected to tools or grippers
+  -->
+  <link name="link_9">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_9.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/link_9.stl" scale="0.2 0.2 0.2"/>  <!-- Use original mesh size -->
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="5.0"/>  <!-- Mass (kg) - ninth joint weight -->
+      <inertia ixx="0.05" ixy="0.0" ixz="0.0" iyy="0.05" iyz="0.0" izz="0.06"/>  <!-- Ninth joint inertia - 0.2x scaled match -->
+    </inertial>
+  </link>
+  
+  <!-- 
+  =============================================================================
+  Debugging tips:
+  =============================================================================
+  
+  1. Joint axis adjustment:
+     - If joint movement direction is wrong, modify <axis xyz="x y z"/>
+     - X-axis(1 0 0): red axis, forward-backward swing
+     - Y-axis(0 1 0): green axis, left-right swing  
+     - Z-axis(0 0 1): blue axis, rotational movement
+  
+  2. Joint position adjustment:
+     - Modify <origin xyz="x y z" rpy="r p y"/>
+     - xyz controls joint position offset
+     - rpy controls joint direction (rarely needs modification)
+  
+  3. Mesh direction adjustment:
+     - If mesh display direction is wrong, modify rpy in link visual
+     - For example: rpy="1.57 0 0" means rotate 90 degrees around X-axis
+  
+  4. Joint limit adjustment:
+     - Modify <limit lower="min_angle" upper="max_angle"/>
+     - Angle unit is radians: pi approximately 3.14159 approximately 180 degrees
+  
+  5. Common problems:
+     - If robot arm collapses: check if mass is 0
+     - If movement is jerky: adjust damping value
+     - If joint doesn't move: check joint limits and axis settings
+  -->
+  
+</robot>

requirements.txt

@@ -0,0 +1,6 @@
+pybullet>=3.2.5
+PyKDL>=1.5.1
+urdf_parser_py>=0.0.4
+numpy>=1.21.0
+scipy>=1.7.0
+matplotlib>=3.5.0