gazebo显示完成

This commit is contained in:
2026-02-24 22:53:50 +08:00
parent e781a6e42f
commit 23b51d404e
61 changed files with 1801 additions and 120 deletions

View File

@@ -1 +1 @@
/home/quella/ROS2_WS/simulation_ws/robotic_arm/install/robotic_arm:/opt/ros/humble
/opt/ros/humble

View File

@@ -0,0 +1,31 @@
controller_manager:
ros__parameters:
update_rate: 50
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
arm_position_controller:
type: joint_trajectory_controller/JointTrajectoryController
arm_position_controller:
ros__parameters:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
command_interfaces:
- position
state_interfaces:
- position
- velocity
gains:
joint1: { p: 100.0, d: 10.0, i: 0.01 }
joint2: { p: 100.0, d: 10.0, i: 0.01 }
joint3: { p: 100.0, d: 10.0, i: 0.01 }
joint4: { p: 100.0, d: 10.0, i: 0.01 }
joint5: { p: 100.0, d: 10.0, i: 0.01 }
joint6: { p: 100.0, d: 10.0, i: 0.01 }

View File

@@ -0,0 +1,64 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, TimerAction
from launch_ros.actions import Node
def generate_launch_description():
pkg_path = get_package_share_directory('robotic_arm')
xacro_file = os.path.join(pkg_path, 'urdf', 'robotic_arm.urdf.xacro')
robot_description_config = xacro.process_file(xacro_file)
robot_description = {'robot_description': robot_description_config.toxml()}
# 启动 Gazebo
gazebo = ExecuteProcess(
cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so'],
output='screen'
)
# 发布机器人状态
rsp = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
# 在 Gazebo 中生成机器人
spawn = Node(
package='gazebo_ros',
executable='spawn_entity.py',
output='screen',
arguments=[
'-topic', 'robot_description',
'-entity', 'robotic_arm',
'-x', '0',
'-y', '0',
'-z', '0.0',
],
)
# 加载关节状态控制器
spawn_jsc = Node(
package='controller_manager',
executable='spawner',
output='screen',
arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
)
# 加载机械臂位置控制器
spawn_arm = Node(
package='controller_manager',
executable='spawner',
output='screen',
arguments=['arm_position_controller', '--controller-manager', '/controller_manager'],
)
return LaunchDescription([
gazebo,
rsp,
TimerAction(period=3.0, actions=[spawn]),
TimerAction(period=6.0, actions=[spawn_jsc, spawn_arm]),
])

View File

@@ -1,6 +1,5 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 颜色 -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0" />
</material>

View File

@@ -2,7 +2,14 @@
<robot name="robotic_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find robotic_arm)/urdf/materials.xacro" />
<!-- 底座连杆 -->
<link name="world" />
<joint name="world_to_base" type="fixed">
<parent link="world" />
<child link="base_link" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="base_link">
<visual>
<geometry>
@@ -24,18 +31,16 @@
</inertial>
</link>
<!-- 关节1:底座旋转 -->
<joint name="joint1" type="revolute">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
<parent link="base_link" />
<child link="link1" />
<axis xyz="0.0 0.0 1.0" />
<limit lower="-3.14159" upper="3.14159" effort="100.0" velocity="1.0" />
<!-- 阻尼系数和摩擦系数,模拟运动阻力和摩擦力 -->
<dynamics damping="0.1" friction="0.1" />
</joint>
<!-- 连杆1 -->
<link name="link1">
<visual>
<geometry>
@@ -57,18 +62,18 @@
</inertial>
</link>
<!-- 关节2:肩部俯仰 -->
<joint name="joint2" type="revolute">
<parent link="link1" />
<child link="link2" />
<origin xyz="0.0 0.0 0.15" rpy="0.0 0.0 0.0" />
<axis xyz="0.0 1.0 0.0" />
<limit lower="-1.5708" upper="1.5708" effort="100.0" velocity="1.0" />
<!-- 阻尼系数和摩擦系数,模拟运动阻力和摩擦力 -->
<dynamics damping="0.1" friction="0.1" />
</joint>
<!-- 连杆2 -->
<link name="link2">
<visual>
<geometry>
@@ -90,7 +95,7 @@
</inertial>
</link>
<!-- 关节3: 肘部俯仰 -->
<joint name="joint3" type="revolute">
<parent link="link2" />
<child link="link3" />
@@ -100,7 +105,7 @@
<dynamics damping="0.1" friction="0.1" />
</joint>
<!-- 连杆3 -->
<link name="link3">
<visual>
<geometry>
@@ -122,7 +127,7 @@
</inertial>
</link>
<!-- 关节4: 腕部滚转 -->
<joint name="joint4" type="revolute">
<parent link="link3" />
<child link="link4" />
@@ -132,7 +137,7 @@
<dynamics damping="0.05" friction="0.05" />
</joint>
<!-- 连杆4 -->
<link name="link4">
<visual>
<geometry>
@@ -154,7 +159,7 @@
</inertial>
</link>
<!-- 关节5: 腕部俯仰 -->
<joint name="joint5" type="revolute">
<parent link="link4" />
<child link="link5" />
@@ -164,7 +169,7 @@
<dynamics damping="0.05" friction="0.05" />
</joint>
<!-- 连杆5 -->
<link name="link5">
<visual>
<geometry>
@@ -186,7 +191,6 @@
</inertial>
</link>
<!-- 关节6: 腕部偏航 -->
<joint name="joint6" type="revolute">
<parent link="link5" />
<child link="link6" />
@@ -196,7 +200,7 @@
<dynamics damping="0.05" friction="0.05" />
</joint>
<!-- 连杆6 (末端执行器) -->
<link name="link6">
<visual>
<geometry>
@@ -218,7 +222,7 @@
</inertial>
</link>
<!-- 末端执行器固定关节 -->
<joint name="end_effector_fixed" type="fixed">
<parent link="link6" />
<child link="end_effector" />
@@ -227,4 +231,94 @@
<link name="end_effector" />
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find robotic_arm)/config/controllers.yaml</parameters>
</plugin>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="link1">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link2">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link3">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link4">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link5">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link6">
<material>Gazebo/Grey</material>
</gazebo>
<ros2_control name="robotic_arm" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-3.14159</param>
<param name="max">3.14159</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1.5708</param>
<param name="max">1.5708</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="joint3">
<command_interface name="position">
<param name="min">-1.5708</param>
<param name="max">1.5708</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="joint4">
<command_interface name="position">
<param name="min">-3.14159</param>
<param name="max">3.14159</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="joint5">
<command_interface name="position">
<param name="min">-1.5708</param>
<param name="max">1.5708</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="joint6">
<command_interface name="position">
<param name="min">-3.14159</param>
<param name="max">3.14159</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
</ros2_control>
</robot>