kuka七自由度机械臂的gazebo仿真
仿真环境
仿真环境：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.0
forward_position_controller:
ros__parameters:
joints:
- A1
- A2
- A3
- A4
- A5
- A6
- A7
interface_name: position # 控制接口 position 状态接口 position
controller_config.yaml (控制器配置文件)
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import IncludeLaunchDescription
def 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 LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessStart
def 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 kinematics
using 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 dependencies
find_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_node
add_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_node
add_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
724
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
