Doni/RoboticArmTest
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
master
RoboticArmTest/models/manual_robot.urdf
sladro 3b5306611a Implement KDL kinematics engine and complete core framework 
Major Features Added:
- KDL-based kinematics engine with world/local coordinate transformation
- Complete GUI system with configuration management
- Robot loader with URDF parsing and joint control
- Enhanced environment system with transport object management
- Main application entry point with dependency validation

Technical Improvements:
- World coordinate to robot base coordinate transformation in KDL
- Real-time configuration updates through GUI
- Comprehensive error handling and validation
- Configuration-driven design throughout
- Robot model scaling adjusted to 0.2x for proper visualization

Files Added:
- src/robot/kinematics.py: KDL forward/inverse kinematics with coordinate transforms
- src/gui/: Complete GUI framework with main window and config management
- main.py: Application entry point with environment validation
- models/*.stl: Robot link mesh files for URDF visualization
- tests/: Unit test framework for core components

This commit establishes the complete foundation for robotic arm path planning
and task execution development.

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

Co-Authored-By: Claude <noreply@anthropic.com>
2025-09-10 14:37:27 +08:00

507 lines
21 KiB
XML
Raw Permalink Blame History

<!--
=============================================================================
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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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="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>
Reference in New Issue View Git Blame Copy Permalink