kuka七自由度机械臂的gazebo仿真
- 仿真环境
- 仿真的ros功能包的文件架构
- 第一个功能包lbr_simulation_gazebo文件详细介绍
- 第二个功能包lbr_simulation_gazebo_command文件详细介绍
- 补充：用于仿真的ros包创建及功能包下载
仿真环境
仿真环境：Ubuntu 22.04(见Fig.1), ROS2 humble(见Fig.2), gazebo 11(见Fig.3)。
Fig.1 ubuntu版本
Fig.2 ROS2版本
Fig.3 Gazebo版本
仿真的ros功能包的文件架构
为了解耦文件，不让不同功能的文件混在一起，这里写了两个ros功能包。两个功能包的文件架构分别如Fig. 4和Fig. 5所示。
Fig. 4 功能包lbr_simulation_gazebo的文件架构
Fig.5 功能包lbr_simulation_gazebo_command的文件架构
简要介绍两个功能包的作用，第一个ros功能包lbr_simulation_gazebo的作用是将my_iiwa7.urdf.xacro文件中定义的机器人模型加载到gazebo仿真环境中。并且将ros2 control与gazebo仿真环境中的机器人关联起来，并且提供了ros topic接口用于接收用户发送的位置控制指令。
第二个ros功能包lbr_simulation_gazebo_command的作用是定义了ros节点，用于供用户给机器人发送位置控制命令。
第一个功能包lbr_simulation_gazebo文件详细介绍
controller_manager: # ros_parameters后面的参数包括：更新频率update_rate, 状态播报器joint_state_broadcaster, # 控制器joint_trajectory_controllers, forward_position_controller，这些在启动 # controller_manager时会传递给controller_manager ros__parameters: update_rate: 100 # controller manager更新频率为100 Hz joint_state_broadcaster: # 用于播报gazebo仿真中的机器人的关节状态，比如joint position type: joint_state_broadcaster/JointStateBroadcaster # 播报器类型，由ros2 controller提供的标准类型 publish_rate: 100 # 播报频率为100 Hz joint_trajectory_controller: # 关节轨迹控制器 type: joint_trajectory_controller/JointTrajectoryController # 控制器类型，由ros2 controller提供的标准类型 forward_position_controller: # 前向位置控制器 type: position_controllers/JointGroupPositionController # 控制器类型，由ros2 controller提供的标准类型# joint_trajectory_controller的具体定义，包括关节名称，控制接口和状态接口joint_trajectory_controller: ros__parameters: joints: # joints后面跟着的A1, A2, ..., A7都是关节的名称，一共7个关节 - A1 - A2 - A3 - A4 - A5 - A6 - A7 command_interfaces: # 控制接口 - position state_interfaces: # 状态接口 - position - velocity state_publish_rate: 50.0 action_monitor_rate: 20.0forward_position_controller: ros__parameters: joints: - A1 - A2 - A3 - A4 - A5 - A6 - A7 interface_name: position # 控制接口 position 状态接口 positioncontroller_config.yaml (控制器配置文件)
import osfrom launch import LaunchDescriptionfrom launch_ros.actions import Nodefrom launch.substitutions import Command, LaunchConfigurationfrom launch.actions import DeclareLaunchArgumentfrom launch_ros.substitutions import FindPackageSharefrom launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch.actions import IncludeLaunchDescriptiondef generate_launch_description(): # 寻找需要的软件包和文件的路径 pkg_share = FindPackageShare(package='lbr_simulation_gazebo').find('lbr_simulation_gazebo') xacro_file = os.path.join(pkg_share, 'urdf', 'my_iiwa7.urdf.xacro') controller_yaml_path = os.path.join(pkg_share, 'config', 'controller_config.yaml') # 设置一个参数，'use_sim_time'为true use_sim_time = LaunchConfiguration('use_sim_time', default='true') # 解析机器人描述文件，存储在robot_description robot_description = Command(['xacro ', xacro_file, ' ', 'controller_yaml_path:=', controller_yaml_path]) # xacro_file是存储my_iiwa7.urdf.xacro的路径, controller_yaml_path是控制器配置文件的路径 # 封装成键值对，为了方便后续作为参数传入节点 robot_description_param = {'robot_description': robot_description} # gazebo.launch.py是gazebo_ros包中标配的文件，是一个空的环境 gazebo_launch_file = os.path.join(FindPackageShare('gazebo_ros').find('gazebo_ros'), 'launch', 'gazebo.launch.py') return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', default_value='true', description='Use simulation (Gazebo) clock if true' ), # 在gazebo中加载一个空的环境，作为gazebo背景环境 IncludeLaunchDescription( PythonLaunchDescriptionSource(gazebo_launch_file) ), # robot_state_publisher这个节点很重要，没有这个节点，下一个Node # executable='spawn_entity.py'启动时找不到'/robot_description'这个ros topic # 这是因为robot_state_publisher发布了ros topic '/robot_description'(type:std_msgs/msg/String) # robot_state_publisher这个节点订阅了ros topic '/joint_states'(Type: sensor_msgs/msg/JointState), 然后基于这个计算出机器人的实时坐标系变换关系，再将其发布到ros topic '/tf'上供其他ros节点订阅 Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', parameters=[robot_description_param, {'use_sim_time': use_sim_time}] ), # 下面这个节点解析了 '/robot_description'中的机器人描述语言， 然后将机器人模型加载到gazebo环境中。 # 注意由于下面这几行 # <!-- ros_control-plugin --> # <gazebo> # <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"> # <parameters>/home/nearlab-iiwa/new-lbr-stack/install/lbr_simulation_gazebo/share/lbr_simulation_gazebo/config/controller_config.yaml</parameters> # </plugin> # </gazebo> # 存在于机器人的URDF文件中(my_iiwa7.urdf.xacro), ros节点 '/controller_manager'(通常情况下通过运行package='controller_manager', executable='ros2_control_node'来启动) 将会自动启动当运行下面这个节点时(package='gazebo_ros' executable='spawn_entity.py') Node( package='gazebo_ros', executable='spawn_entity.py', name='spawn_entity', output='screen', arguments=['-topic', 'robot_description', '-entity', 'my_robot'], # '-topic' refers to the ros topic which contains robot description parameters=[robot_description_param, {'use_sim_time': use_sim_time}] ), # 'ros2_control_node' 是controller manager的核心节点（运行后将出现一个名为'\controller_manager'的ros node）, 负载加载和运行所有的ros控制器。 # 换句话说，所有ros controller的节点的加载和运行依赖于'\controller_manager'这个节点。 # 因此这个节点('\controller_manager')应该在其它ros控制器节点加载和运行之前存在。 # 但是，由于此文件前面启动的节点，已经自动启动'\controller_manager'这个节点了，所以这里注释了下面这几行代码。 # Node( # package='controller_manager', # executable='ros2_control_node', # parameters=[controller_yaml, {'use_sim_time': use_sim_time}], # output='screen' # ), # (package = 'controller_manager' executable = 'spawner')是一个工具类程序, # 用于发送一个请求给'\controller_manager'节点来加载和运行我们想启动的ros控制器 #（当然，这个想启动的ros控制器必须在控制器配置文件中必须已经定义过，我们这里的控制器配置文件是controller_config.yaml） Node( package='controller_manager', executable='spawner', # '--controller-manager' 是固定写法，无特殊含义 # '/controller_manager'是前面已经启动的ros控制器管理节点 # 这里，executable='spawner'与ros控制器管理节点'/controller_manager'通信请求运行'joint_state_broadcaster'这个状态播报器 #（状态播报器可以理解为广义的ros控制器，由ros controller包中自行提供的标准控制器） arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'], output='screen' ),# 下面这几行也是请求控制器加载，我们先把这个给注释了。 # Node( # package='controller_manager', # executable='spawner', # arguments=['joint_trajectory_controller', '--controller-manager', '/controller_manager'], # output='screen' # ),# 请求'\controller_manager'节点加载和运行'forward_position_controller'控制器。 Node( package='controller_manager', executable='spawner', arguments=['forward_position_controller', '--controller-manager', '/controller_manager'], output='screen' ) ])if __name__ == '__main__': generate_launch_description()load_iiwa7_gazebo.launch.py （将机器人模型导入到gazebo中，并创建ros控制器用于控制gazebo中的机器人模型）
<?xml version="1.0" ?><!-- =================================================================================== --><!-- | This document was autogenerated by xacro from /home/nearlab-iiwa/new-lbr-stack/src/lbr_fri_ros2_stack/lbr_description/urdf/iiwa7/iiwa7.xacro | --><!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --><!-- =================================================================================== --><!-- top level --><robot name="iiwa7"> <!-- xacro:arg支持从外部程序中传入配置参数（如文件路径） --> <xacro:arg name="controller_yaml_path" default="please_input_your_default_path/lbr_simulation_gazebo/config/controller_config.yaml" /> <link name="world"/> <!--joint between world and link_0--> <joint name="world_joint" type="fixed"> <parent link="world"/> <child link="link_0"/> </joint> <!-- ros_control-plugin --> <!-- ros2 control中的Gazebo plugin(Gazebo插件)是用来连接ros2 control和gazebo仿真中的机器人硬件接口（包括控制接口和状态接口） --> <!-- 将gazebo仿真中的机器人硬件接口，比如关节位置，关节速度和关节力矩，封装集成到ros2 control中, 使得ros控制器可以控制gazebo仿真中的机器人。 --> <!-- 具体细节作用如下， --> <!-- 1. 这个插件作为桥梁, 将gazebo仿真中的机器人硬件接口数据(如关节位置, 速度和力) 传递给ros控制器，同时将ros控制器的控制命令发送给gazebo仿真中的机器人的硬件接口。--> <!-- 2. 这个插件还能使用这个URDF文件中定义的机器人硬件接口作为gazebo仿真中的机器人的硬件接口。这些硬件接口包括了机器人关节的控制接口和状态接口，其中控制接口用于接收ros控制器发送的命令将其应用于gazebo仿真中的机器人关节，状态接口用于给ros控制器反馈gazebo仿真中的机器人关节状态数据。 --> <!-- 注意: 我们可以将该插件定义在机器人模型的URDF文件中， 这个插件会自动检测URDF文件中定义的gazebo仿真中的机器人的硬件接口。这个插件同时可以将ros控制器中提供的接口与gazebo仿真环境中的机器人的硬件接口匹配。 --> <gazebo> <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"> <parameters>$(arg controller_yaml_path)</parameters> </plugin> </gazebo> <!-- 下面这些标签定义了gazebo仿真环境下的机器人的杆件的表面摩擦系数等，为了仿真更真实 --> <gazebo reference="link_0"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A1"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_1"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A2"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_2"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A3"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_3"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A4"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_4"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A5"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_5"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A6"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_6"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <gazebo reference="A7"> <implicitSpringDamper>true</implicitSpringDamper> <provideFeedback>true</provideFeedback> </gazebo> <gazebo reference="link_7"> <mu1>0.2</mu1> <mu2>0.2</mu2> </gazebo> <!-- 定义了gazebo环境中的机器人的硬件接口，为了和ros control框架交互 --> <!-- 注意：我们也可以定义硬件接口在.yaml文件中(如controll_config.yaml) --> <ros2_control name="system_interface" type="system"> <hardware> <plugin>gazebo_ros2_control/GazeboSystem</plugin> </hardware> <joint name="A1"> <param name="min_position">-2.9670597283903604</param> <param name="max_position">2.9670597283903604</param> <param name="max_velocity">1.710422666954443</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.9670597283903604</param> <param name="max">2.9670597283903604</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A2"> <param name="min_position">-2.0943951023931953</param> <param name="max_position">2.0943951023931953</param> <param name="max_velocity">1.710422666954443</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.0943951023931953</param> <param name="max">2.0943951023931953</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A3"> <param name="min_position">-2.9670597283903604</param> <param name="max_position">2.9670597283903604</param> <param name="max_velocity">1.7453292519943295</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.9670597283903604</param> <param name="max">2.9670597283903604</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A4"> <param name="min_position">-2.0943951023931953</param> <param name="max_position">2.0943951023931953</param> <param name="max_velocity">2.2689280275926285</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.0943951023931953</param> <param name="max">2.0943951023931953</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A5"> <param name="min_position">-2.9670597283903604</param> <param name="max_position">2.9670597283903604</param> <param name="max_velocity">2.443460952792061</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.9670597283903604</param> <param name="max">2.9670597283903604</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A6"> <param name="min_position">-2.0943951023931953</param> <param name="max_position">2.0943951023931953</param> <param name="max_velocity">3.141592653589793</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-2.0943951023931953</param> <param name="max">2.0943951023931953</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> <joint name="A7"> <param name="min_position">-3.0543261909900763</param> <param name="max_position">3.0543261909900763</param> <param name="max_velocity">3.141592653589793</param> <param name="max_torque">200</param> <command_interface name="position"> <param name="min">-3.0543261909900763</param> <param name="max">3.0543261909900763</param> </command_interface> <command_interface name="effort"> <param name="min">-200</param> <param name="max"> 200</param> </command_interface> <state_interface name="position"/> <state_interface name="velocity"/> <state_interface name="effort"/> </joint> </ros2_control> <!-- 定义了机器人的link和joint，这些才是传统的机器人的URDF文件，不涉及gazebo的内容 --> <link name="link_0"> <inertial> <origin rpy="0 0 0" xyz="-0.012857 0.0 0.069964"/> <mass value="4.855658"/> <inertia ixx="0.017839" ixy="0.0" ixz="0.000781" iyy="0.022294" iyz="0.0" izz="0.021334"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_0.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_0.stl"/> </geometry> </collision> </link> <joint name="A1" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0 0.1475"/> <parent link="link_0"/> <child link="link_1"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="1.710422666954443"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_1"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.034819 0.123299"/> <mass value="3.394011"/> <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="0.003797" izz="0.007563"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_1.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_1.stl"/> </geometry> </collision> </link> <joint name="A2" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0105 0.1925"/> <parent link="link_1"/> <child link="link_2"/> <axis xyz="0.0 1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.710422666954443"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_2"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.039793 0.086944"/> <mass value="4.031991"/> <inertia ixx="0.031697" ixy="0.0" ixz="0.0" iyy="0.03008" iyz="0.005889" izz="0.009666"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_2.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_2.stl"/> </geometry> </collision> </link> <joint name="A3" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0105 0.2075"/> <parent link="link_2"/> <child link="link_3"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="1.7453292519943295"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_3"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.034819 0.123299"/> <mass value="3.394011"/> <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="-0.003797" izz="0.007563"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_3.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_3.stl"/> </geometry> </collision> </link> <joint name="A4" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0105 0.1925"/> <parent link="link_3"/> <child link="link_4"/> <axis xyz="0.0 -1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="2.2689280275926285"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_4"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.039793 0.086944"/> <mass value="4.031989"/> <inertia ixx="0.031695" ixy="0.0" ixz="0.0" iyy="0.030079" iyz="-0.005889" izz="0.009665"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_4.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_4.stl"/> </geometry> </collision> </link> <joint name="A5" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0105 0.2075"/> <parent link="link_4"/> <child link="link_5"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="2.443460952792061"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_5"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.029824 0.076267"/> <mass value="1.529239"/> <inertia ixx="0.008485" ixy="0.0" ixz="0.0" iyy="0.007136" iyz="0.002806" izz="0.003848"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_5.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_5.stl"/> </geometry> </collision> </link> <joint name="A6" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0707 0.1925"/> <parent link="link_5"/> <child link="link_6"/> <axis xyz="0.0 1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="3.141592653589793"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_6"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.07102 0.00495"/> <mass value="2.403626"/> <inertia ixx="0.007067" ixy="0.0" ixz="0.0" iyy="0.006804" iyz="0.000311" izz="0.004629"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_6.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_6.stl"/> </geometry> </collision> </link> <joint name="A7" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0707 0.091"/> <parent link="link_6"/> <child link="link_7"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-3.0543261909900763" upper="3.0543261909900763" velocity="3.141592653589793"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_7"> <inertial> <origin rpy="0 0 0" xyz="3.0000e-06 -2.0000e-06 1.3782e-02"/> <mass value="0.259474"/> <inertia ixx="0.000171" ixy="0.0" ixz="0.0" iyy="0.000171" iyz="0.0" izz="0.000299"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -1.231"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_7.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -1.231"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_7.stl"/> </geometry> </collision> </link> <joint name="joint_ee" type="fixed"> <parent link="link_7"/> <child link="link_ee"/> <origin rpy="0 0 0" xyz="0 0 0.035"/> </joint> <link name="link_ee"> </link></robot>my_iiwa7.urdf.xacro （定义了机器人的URDF，以及在gazebo仿真中与ros controller交互的硬件接口）
cmake_minimum_required(VERSION 3.8)project(lbr_simulation_gazebo)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic)endif()find_package(ament_cmake REQUIRED)find_package(rclcpp REQUIRED)find_package(rclpy REQUIRED)find_package(urdf REQUIRED)find_package(xacro REQUIRED)find_package(gazebo_ros2_control REQUIRED)find_package(gazebo_ros REQUIRED)find_package(controller_manager REQUIRED)find_package(control_toolbox REQUIRED)find_package(ros2_control REQUIRED)find_package(ros2_controllers REQUIRED)find_package(robot_state_publisher REQUIRED)include_directories( include ${rclcpp_INCLUDE_DIRS} ${gazebo_ros2_control_INCLUDE_DIRS} ${gazebo_ros_INCLUDE_DIRS} ${controller_manager_INCLUDE_DIRS} ${control_toolbox_INCLUDE_DIRS} ${ros2_control_INCLUDE_DIRS} ${ros2_controllers_INCLUDE_DIRS})install( DIRECTORY launch urdf config DESTINATION share/${PROJECT_NAME})ament_package()CMakeLists.txt (配置文件)
<?xml version="1.0"?><?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3"> <name>lbr_simulation_gazebo</name> <version>0.0.0</version> <description>TODO: Package description</description> <maintainer email="nearlab-iiwa@todo.todo">nearlab-iiwa</maintainer> <license>TODO: License declaration</license> <buildtool_depend>ament_cmake</buildtool_depend> <build_depend>rclpy</build_depend> <build_depend>rclcpp</build_depend> <build_depend>xacro</build_depend> <build_depend>urdf</build_depend> <build_depend>gazebo_ros2_control</build_depend> <build_depend>gazebo_ros</build_depend> <build_depend>controller_manager</build_depend> <build_depend>control_toolbox</build_depend> <build_depend>ros2_control</build_depend> <build_depend>ros2_controllers</build_depend> <build_depend>robot_state_publisher</build_depend> <exec_depend>rclcpp</exec_depend> <exec_depend>rclpy</exec_depend> <exec_depend>xacro</exec_depend> <exec_depend>urdf</exec_depend> <exec_depend>gazebo_ros2_control</exec_depend> <exec_depend>gazebo_ros</exec_depend> <exec_depend>controller_manager</exec_depend> <exec_depend>control_toolbox</exec_depend> <exec_depend>ros2_control</exec_depend> <exec_depend>ros2_controllers</exec_depend> <exec_depend>robot_state_publisher</exec_depend> <export> <build_type>ament_cmake</build_type> </export></package>package.xml
当我们运行load_iiwa7_gazebo.launch.py这个文件时，创建了Fig. 6所示gazebo与ros2 control之间的通信逻辑（下图并没有展示所有的细节逻辑，整体逻辑如下图所示）。
Fig. 6 gazebo中的机器人与ros2 control的通信
Fig. 6中的libgazebo_ros2_control.so这个插件（plugin）的作用很重要，当我们运行load_iiwa7_gazebo.launch.py文件中的
**Node(
package=‘gazebo_ros’,
executable=‘spawn_entity.py’,
name=‘spawn_entity’,
output=‘screen’,
arguments=[‘-topic’, ‘robot_description’, ‘-entity’, ‘my_robot’], # ‘-topic’ refers to the ros topic which contains robot description
parameters=[robot_description_param, {‘use_sim_time’: use_sim_time}]
)**这个节点时，会运行"/robot_description"中的my_iiwa7.urdf.xacro文件中的libgazebo_ros2_control.so这个插件，将ros2 controller的接口集成（桥接）到gazebo中的机器人硬件接口上面。举例说明一下这个过程，比如ros2 controller的控制频率是100 Hz, 当控制周期到来时，ros2 controller会将计算出来的控制命令（比如关节位置，关节速度或者关节力矩）通过libgazebo_ros2_control.so这个插件（plugin）发送给gazebo，gazebo收到控制命令后，就会将这些控制命令应用到gazebo仿真环境中的机器人上面，gazebo物理引擎通过计算来更新gazebo仿真环境中的机器人的状态（比如关节位置，关节力等）。
运行完load_iiwa7_gazebo.launch.py这个文件后，我们就可以在gazebo中看见机械臂了，如Fig. 7所示。并且可以通过ros2 topic list和ros2 node list查看正在运行的ros话题和ros节点，如Fig. 8所示。
Fig.7 gazebo中的机械臂
Fig.8 ros topic和ros node
第二个功能包lbr_simulation_gazebo_command文件详细介绍
from launch import LaunchDescriptionfrom launch_ros.actions import Nodefrom launch.actions import ExecuteProcess, RegisterEventHandlerfrom launch.event_handlers import OnProcessStartdef generate_launch_description(): # move robot to initial position, which is far away from singular pose robot_to_initial_position = ExecuteProcess( cmd=['ros2', 'topic', 'pub', '--once', '/forward_position_controller/commands', 'std_msgs/msg/Float64MultiArray', '{data: [0.0, 0.02, 0.0, 1.57, 0.0, -1.57, 0.0]}'], output='screen' ) cartesian_pose_node = Node( package='lbr_simulation_gazebo_command', executable='cartesian_pose_node', name='cartesian_pose_node', output='screen' ) cartesian_path_planning_node = Node( package='lbr_simulation_gazebo_command', executable='cartesian_path_planning_node', name='cartesian_path_planning_node', output='screen' )# we use 'TimerAction' to ensure that ros nodes 'robot_to_initial_position', 'cartesian_pose_node', 'cartesian_path_planning_node' can start in order return LaunchDescription([robot_to_initial_position, TimerAction( period = 1.0, # delay 1 second and then start 'cartesian_pose_node' actions=[cartesian_pose_node] ), TimerAction( period = 2.0, # delay 2 seconds and then start 'cartesian_path_planning_node' actions = [cartesian_path_planning_node] ) ])cartesian_pose_control.launch.py（作用：首先，将机械臂运行到初始位置，即一个非奇异的位姿。然后，启动’cartesian_pose_node’节点，用于给ros2 controller发送关节空间的控制命令，同时接收gazebo中机械臂的关节空间的状态信息，比如关节位置等。最后，启动’cartesian_path_planning_node’节点，发送笛卡尔空间的位姿命令和接收机械臂笛卡尔空间的位姿状态。）
#include "rclcpp/rclcpp.hpp"#include <iostream>#include <fstream>#include <sstream>#include <string>#include <filesystem>#include "geometry_msgs/msg/pose.hpp" // for describing Cartesian Pose#include "sensor_msgs/msg/joint_state.hpp"#include "std_msgs/msg/float64_multi_array.hpp"#include "kdl_parser/kdl_parser.hpp"#include "kdl/chainfksolverpos_recursive.hpp" // for forward kinematics#include "kdl/chainiksolverpos_lma.hpp" // for inverse kinematicsusing std::placeholders::_1;using namespace std::chrono_literals;class CartesianPoseNode:public rclcpp::Node{ private:rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr joint_position_publisher_; rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_position_subscriber_; rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_; rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_; sensor_msgs::msg::JointState current_robot_state_; // robot state, including joint positions geometry_msgs::msg::Pose current_cartesian_position_; // current cartesian pose of robot KDL::Chain chain_; // robot kinematics chain exetracted from robot URDF file private: /** * @function: callback function for Joint Position Subscriber * @param msg joint position of the robot */ void joint_position_sub_callback(const sensor_msgs::msg::JointState& msg) { current_robot_state_ = msg; /* it is weired that when subscribing robot joint states from ros topic '/joint_states' in ros node '/joint_state_broadcaster', the order of joint 'A3' and 'A4' is wrong. Thus, here we adjust it manually to the right order. */ auto temp = current_robot_state_.position[2]; current_robot_state_.position[2] = current_robot_state_.position[3]; current_robot_state_.position[3] = temp; double joint_position[7]; for(int i = 0; i < 7; i++) { joint_position[i] = current_robot_state_.position[i]; //std::cout << current_robot_state_.position[i] << std::endl; } current_cartesian_position_ = computeForwardKinematics(joint_position); cartesian_pose_publisher_->publish(current_cartesian_position_); return; } /** * @function: callback function for Cartesian Pose Subscriber * @param msg cartesian pose command */ void cartesian_pose_sub_callback(const geometry_msgs::msg::Pose& msg) { unsigned int joint_number = chain_.getNrOfJoints(); // for kuka robot, 7 joints KDL::JntArray current_joint_positions(joint_number); std_msgs::msg::Float64MultiArray joint_position_command; joint_position_command.data.resize(7); for(unsigned int i = 0; i < joint_number; i++) { current_joint_positions(i) = current_robot_state_.position[i]; } joint_position_command = computeInverseKinematics(msg, current_joint_positions); joint_position_publisher_->publish(joint_position_command); return; } public: CartesianPoseNode():Node("cartesian_pose_node") { // get the path of urdf file std::string urdf_file_path = "/home/nearlab-iiwa/new-lbr-stack/src/lbr_fri_ros2_stack/lbr_simulation_gazebo_command/urdf/iiwa7.urdf"; // path of your robot urdf file std::string robot_description_string = readUrdfFile(urdf_file_path); KDL::Tree robot_tree; if(!kdl_parser::treeFromString(robot_description_string, robot_tree)) { std::cout << "Failed to construct kdl tree." << std::endl; } std::string root_link = "link_0"; // adjust with your URDF‘s root link std::string tip_link = "link_ee"; // adjust with your URDF‘s tip link if(!robot_tree.getChain(root_link, tip_link, chain_)) { std::cerr << "Failed to get chain from tree." << std::endl; } else { std::cout << "Get chain from tree successfully." << std::endl; } joint_position_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>( "/forward_position_controller/commands", 10); joint_position_subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>( "/joint_states", 10, std::bind(&CartesianPoseNode::joint_position_sub_callback, this, _1)); cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>( "/lbr/state/cartesian_pose", 10); cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>( "/lbr/command/cartesian_pose", 10, std::bind(&CartesianPoseNode::cartesian_pose_sub_callback, this, _1)); } /** * @function: convert URDF file to a string * @param urdf_file_path the path of URDF file * @return string type of URDF file */ std::string readUrdfFile(const std::string& urdf_file_path) { std::ifstream file_stream(urdf_file_path); if(!file_stream) // if open this file failed, return null string { std::cerr << "Failed to open file at path:" << urdf_file_path << std::endl; return ""; } std::stringstream buffer; buffer << file_stream.rdbuf(); return buffer.str(); } /** * @function: calculate forward kinematics of robot * @param position_array_ptr store seven joint positions of robot * @return cartesian pose of the robot */ geometry_msgs::msg::Pose computeForwardKinematics(double* position_array_ptr) { KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_); unsigned int joint_number = chain_.getNrOfJoints(); KDL::JntArray joint_positions = KDL::JntArray(joint_number); for(unsigned int i = 0; i < joint_number; i++) { joint_positions(i) = position_array_ptr[i]; } KDL::Frame cartesian_pose_temp; // Cartesian Pose described in data type KDL::Frame geometry_msgs::msg::Pose cartesian_pose; // described in geometry_msgs::msg::Pose if(fk_solver.JntToCart(joint_positions, cartesian_pose_temp) < 0) { std::cerr << "FK Solver to calculate JointToCartesian failed." << std::endl; } else { // Position cartesian_pose.position.x = cartesian_pose_temp.p.x(); cartesian_pose.position.y = cartesian_pose_temp.p.y(); cartesian_pose.position.z = cartesian_pose_temp.p.z(); // Orientation double x, y, z, w; cartesian_pose_temp.M.GetQuaternion(x, y, z, w); // get quaternion cartesian_pose.orientation.x = x; cartesian_pose.orientation.y = y; cartesian_pose.orientation.z = z; cartesian_pose.orientation.w = w; } return cartesian_pose; } /** * @function: calculate inverse kinematics of robot * @param desired_cartesian_pose target cartesian pose we want to transform to joint space * @param current_joint_positions current joint positions * @return joint positions command */ std_msgs::msg::Float64MultiArray computeInverseKinematics( const geometry_msgs::msg::Pose& desired_cartesian_pose, KDL::JntArray& current_joint_positions) { KDL::ChainIkSolverPos_LMA ik_solver(chain_); KDL::JntArray result_joint_positions = KDL::JntArray(chain_.getNrOfJoints()); std_msgs::msg::Float64MultiArray joint_position_command; joint_position_command.data.resize(7); // set array size for variable 'joint_position_command', otherwise, the size will be zero. // transfer data type 'geometry::msg::Pose' to be 'KDL::Frame' KDL::Vector position(desired_cartesian_pose.position.x, desired_cartesian_pose.position.y, desired_cartesian_pose.position.z); KDL::Rotation rotation =KDL::Rotation::Quaternion(desired_cartesian_pose.orientation.x, desired_cartesian_pose.orientation.y, desired_cartesian_pose.orientation.z, desired_cartesian_pose.orientation.w); KDL::Frame desired_cartesian_pose_temp(rotation, position); int ik_result = ik_solver.CartToJnt(current_joint_positions, desired_cartesian_pose_temp, result_joint_positions); if(ik_result < 0) // if solving failed, 'ik_result' would be less than 0 { std::cerr << "Inverse kinematics failed." << std::endl; } else { //std::cout << "Inverse kinematics success." << std::endl; for(unsigned int i = 0; i < 7; i++) { joint_position_command.data[i] = result_joint_positions(i); } } return joint_position_command; }};int main(int argc, char** argv){ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<CartesianPoseNode>()); rclcpp::shutdown(); return 0;}cartesian_pose_node.cpp（作用1：用于接收笛卡尔空间的位置控制指令’/lbr/command/castesian_pose’话题，并通过KDL库将其求解出关节空间的解，发送给ros控制器的用户命令订阅话题’/forward_position_controller/commands’。作用2：订阅gazebo仿真环境中的机器人的状态发布话题’/joint_states’，并通过KDL库将其求解出笛卡尔空间的位姿状态，通过’/lbr/state/cartesian_pose’话题发送给用户的订阅节点）
#include "rclcpp/rclcpp.hpp"#include "cmath"#include <iostream>#include <string>#include "geometry_msgs/msg/pose.hpp"using std::placeholders::_1;class CartesianPosePublisherNode:public rclcpp::Node{ private: rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_; rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_; geometry_msgs::msg::Pose initial_cartesian_pose_; // robot starts from this pose bool is_init_; double amplitude_; // rad double frequency_; // Hz double sampling_time_; // sampling time for sending position command double phase_; // initial phase geometry_msgs::msg::Pose previous_cmd_pose_; private: /** * @function: callback function for Cartesian Pose Subscriber * @param msg Cartesian Pose of the robot */ void topic_callback(const geometry_msgs::msg::Pose& msg) { if(!is_init_) { initial_cartesian_pose_ = msg; is_init_ = true; } else { geometry_msgs::msg::Pose cartesian_pose_command = initial_cartesian_pose_; phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_; cartesian_pose_command.position.z += amplitude_ * sin(phase_); std::cout << (previous_cmd_pose_.position.z + - msg.position.z) << std::endl; cartesian_pose_publisher_->publish(cartesian_pose_command); previous_cmd_pose_ = cartesian_pose_command; } return; } public: CartesianPosePublisherNode():Node("cartesian_pose_publisher_node") { is_init_ = false; amplitude_ = 0.05; frequency_ = 0.5; sampling_time_ = 0.01; phase_ = 0.0; cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>( "/lbr/command/cartesian_pose", 10); cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>( "/lbr/state/cartesian_pose", 10, std::bind(&CartesianPosePublisherNode::topic_callback, this, _1)); }};int main(int argc, char** argv){ rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<CartesianPosePublisherNode>()); rclcpp::shutdown(); return 0;}cartesian_path_planning_node.cpp（作用1：通过’/lbr/state/cartesian_pose’话题来订阅机器人的末端执行器的笛卡尔空间的位姿态。 作用2：计算笛卡尔空间的位姿控制指令，并通过’/lbr/command/cartesian_pose’来发布机器人末端执行器的位姿控制指令）
<?xml version="1.0" ?><!-- =================================================================================== --><!-- | This document was autogenerated by xacro from /home/nearlab-iiwa/new-lbr-stack/src/lbr_fri_ros2_stack/lbr_simulation_gazebo/urdf/iiwa7.urdf.xacro | --><!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --><!-- =================================================================================== --><robot name="my_robot"> <!-- <link name="world"/> --> <link name="world"/> <joint name="base_joint" type="fixed"> <parent link="world"/> <child link="link_0"/> <origin rpy="0 0 0" xyz="0 0 0"/> <axis xyz="0 0 1"/> </joint> <link name="link_0"> <inertial> <origin rpy="0 0 0" xyz="-0.012857 0.0 0.069964"/> <mass value="4.855658"/> <inertia ixx="0.017839" ixy="0.0" ixz="0.000781" iyy="0.022294" iyz="0.0" izz="0.021334"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_0.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0 0 0"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_0.stl"/> </geometry> </collision> </link> <joint name="A1" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0 0.1475"/> <parent link="link_0"/> <child link="link_1"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="1.710422666954443"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_1"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.034819 0.123299"/> <mass value="3.394011"/> <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="0.003797" izz="0.007563"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_1.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.1475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_1.stl"/> </geometry> </collision> </link> <joint name="A2" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0105 0.1925"/> <parent link="link_1"/> <child link="link_2"/> <axis xyz="0.0 1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.710422666954443"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_2"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.039793 0.086944"/> <mass value="4.031991"/> <inertia ixx="0.031697" ixy="0.0" ixz="0.0" iyy="0.03008" iyz="0.005889" izz="0.009666"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_2.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0105 -0.34"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_2.stl"/> </geometry> </collision> </link> <joint name="A3" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0105 0.2075"/> <parent link="link_2"/> <child link="link_3"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="1.7453292519943295"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_3"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.034819 0.123299"/> <mass value="3.394011"/> <inertia ixx="0.021383" ixy="0.0" ixz="0.0" iyy="0.020403" iyz="-0.003797" izz="0.007563"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_3.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.5475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_3.stl"/> </geometry> </collision> </link> <joint name="A4" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0105 0.1925"/> <parent link="link_3"/> <child link="link_4"/> <axis xyz="0.0 -1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="2.2689280275926285"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_4"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.039793 0.086944"/> <mass value="4.031989"/> <inertia ixx="0.031695" ixy="0.0" ixz="0.0" iyy="0.030079" iyz="-0.005889" izz="0.009665"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_4.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 -0.0105 -0.74"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_4.stl"/> </geometry> </collision> </link> <joint name="A5" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0105 0.2075"/> <parent link="link_4"/> <child link="link_5"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-2.9670597283903604" upper="2.9670597283903604" velocity="2.443460952792061"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_5"> <inertial> <origin rpy="0 0 0" xyz="0.0 -0.029824 0.076267"/> <mass value="1.529239"/> <inertia ixx="0.008485" ixy="0.0" ixz="0.0" iyy="0.007136" iyz="0.002806" izz="0.003848"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_5.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -0.9475"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_5.stl"/> </geometry> </collision> </link> <joint name="A6" type="revolute"> <origin rpy="0 0 0" xyz="0.0 -0.0707 0.1925"/> <parent link="link_5"/> <child link="link_6"/> <axis xyz="0.0 1.0 0.0"/> <limit effort="200" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="3.141592653589793"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_6"> <inertial> <origin rpy="0 0 0" xyz="0.0 0.07102 0.00495"/> <mass value="2.403626"/> <inertia ixx="0.007067" ixy="0.0" ixz="0.0" iyy="0.006804" iyz="0.000311" izz="0.004629"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_6.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0707 -1.14"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_6.stl"/> </geometry> </collision> </link> <joint name="A7" type="revolute"> <origin rpy="0 0 0" xyz="0.0 0.0707 0.091"/> <parent link="link_6"/> <child link="link_7"/> <axis xyz="0.0 0.0 1.0"/> <limit effort="200" lower="-3.0543261909900763" upper="3.0543261909900763" velocity="3.141592653589793"/> <dynamics damping="10.0" friction="0.1"/> </joint> <link name="link_7"> <inertial> <origin rpy="0 0 0" xyz="3.0000e-06 -2.0000e-06 1.3782e-02"/> <mass value="0.259474"/> <inertia ixx="0.000171" ixy="0.0" ixz="0.0" iyy="0.000171" iyz="0.0" izz="0.000299"/> </inertial> <visual> <origin rpy="0 0 0" xyz="0.0 0.0 -1.231"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/visual/link_7.dae"/> </geometry> </visual> <collision> <origin rpy="0 0 0" xyz="0.0 0.0 -1.231"/> <geometry> <mesh filename="package://lbr_description/meshes/iiwa7/collision/link_7.stl"/> </geometry> </collision> </link> <joint name="joint_ee" type="fixed"> <parent link="link_7"/> <child link="link_ee"/> <origin rpy="0 0 0" xyz="0 0 0.035"/> </joint> <link name="link_ee"> </link></robot>iiwa7.urdf（这个文件存储机械臂的link和joint描述，用于KDL库求解机械臂的正运动学和逆运动学使用。其实这个文件是my_iiwa7.urdf.xacro文件中的内容的一部分，为了逻辑解耦，这里又展示了一下。）
cmake_minimum_required(VERSION 3.8)project(lbr_simulation_gazebo_command)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic)endif()# find dependenciesfind_package(ament_cmake REQUIRED)find_package(rclcpp REQUIRED)find_package(tf2_ros REQUIRED)find_package(sensor_msgs REQUIRED)find_package(std_msgs REQUIRED)find_package(urdf REQUIRED)find_package(geometry_msgs REQUIRED)find_package(orocos_kdl_vendor REQUIRED)find_package(kdl_parser REQUIRED)# cartesian_path_planning_nodeadd_executable(cartesian_path_planning_node src/cartesian_path_planning_node.cpp)ament_target_dependencies(cartesian_path_planning_node rclcpp tf2_ros sensor_msgs geometry_msgs)#cartesian_pose_nodeadd_executable(cartesian_pose_node src/cartesian_pose_node.cpp)ament_target_dependencies(cartesian_pose_node rclcpp tf2_ros urdf sensor_msgs orocos_kdl_vendor kdl_parser geometry_msgs)install(TARGETS cartesian_pose_node cartesian_path_planning_node DESTINATION lib/${PROJECT_NAME})install( DIRECTORY launch urdf DESTINATION share/${PROJECT_NAME})ament_package()CMakeLists.txt
<?xml version="1.0"?><?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3"> <name>lbr_simulation_gazebo_command</name> <version>0.0.0</version> <description>TODO: Package description</description> <maintainer email="nearlab-iiwa@todo.todo">nearlab-iiwa</maintainer> <license>TODO: License declaration</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>tf2_ros</depend> <depend>urdf</depend> <depend>geometry_msgs</depend> <depend>sensor_msgs</depend> <depend>std_msgs</depend> <depend>orocos_kdl_vendor</depend> <depend>kdl_parser</depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_common</test_depend> <export> <build_type>ament_cmake</build_type> </export></package>package.xml
当我们运行cartesian_pose_control.launch.py这个文件时，创建了Fig.9所示的ros节点间的通信逻辑。
Fig.9 运行cartesian_pose_control.launch.py的通信逻辑架构
运行cartesian_pose_control.launch.py文件后，我们可以在gazebo中看到机械臂如Video.1所示，末端执行器沿着笛卡尔空间的z轴方向（即竖直方向）往返运动。
我们可以在’cartesian_path_planning_node.cpp’中写一段程序来输出机械臂在笛卡尔空间的z轴方向上的位置跟踪误差，如Fig.10所示，笛卡尔空间的z方向上面的位置跟踪误差约为1mm。
Fig.10 机械臂在笛卡尔空间中的z轴方向的位置跟踪误差
补充：用于仿真的ros包创建及功能包下载
当两个功能包都成功编译后，我们用下面命令运行load_iiwa7_gazebo.launch.py，作用是启动gazebo，并把机械臂模型加载进gazebo仿真环境中。
ros2 launch lbr_simulation_gazebo load_iiwa7_gazebo.launch.py然后，用下面的命令运行cartesian_pose_control.launch.py，作用是控制机器人的末端沿着笛卡尔空间的z轴方向往返运行。
ros2 launch lbr_simulation_gazebo_command cartesian_pose_control.launch.py为了上述两个功能包依赖的文件都有，请先安装一个功能包，参照https://github.com/lbr-stack/lbr_fri_ros2_stack。按照网页中的Quick Start中的内容指示进行安装。
Fig.11 安装指示图
然后，在这个项目的src文件夹下的lbr_fri_ros2_stack文件夹中创建上述的lbr_simulation_gazebo和lbr_simulation_gazebo_command功能包。这两个功能包和功能包中的文件已经打包上传，请参照链接https://download.csdn.net/download/modest_laowang/89352191
仿真演示视频如下所示，
simulation_gazebo
