URDF 机器人建模
URDF 使用 xml 格式描述机器人模型
首先创建一个包来保存文件 ros2 pkg create robot_describes --destination-directory=src --build-type=ament_cmake --license Apache-2.0
基础
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
| <?xml version="1.0"?>
<!-- 机器人名称 -->
<robot name="first_robot">
<!-- 机器人身体部分 -->
<link name="base_link">
<!-- 部件的外观描述 -->
<visual>
<!-- 沿着自己几何中心的偏移和旋转 -->
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<!-- 几何形状 -->
<geometry>
<!-- 圆柱体 -->
<cylinder radius="0.10" length="0.12"/>
</geometry>
<!-- 材质和颜色 -->
<material name="white">
<color rgba="1.0 1.0 1.0 0.5" />
</material>
</visual>
</link>
<!-- 机器人的 IMU ，惯性测量单元 -->
<link name="imu_link">
<!-- 部件的外观描述 -->
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- 正方体 -->
<box size="0.02 0.02 0.02" />
</geometry>
<!-- 材质和颜色 -->
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 机器人关节，用来组合机器人部件 -->
<joint name="imu_joint" type="fixed">
<!-- 父部件 -->
<parent link="base_link"/>
<!-- 子部件固定到父部件上 -->
<child link="imu_link"/>
<!-- 固定的位置 -->
<origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
</joint>
</robot>
|
使用 xacro 简化 urdf
xacro 工具可以将 xacro 描述文件转成 xml 文件，xacro 对比原生 xml 提供了一些方便的功能，比如宏
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
| <?xml version="1.0"?>
<!-- 机器人名称 -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
<xacro:macro name="base_link" params="length radius">
<!-- 机器人身体部分 -->
<link name="base_link">
<!-- 部件的外观描述 -->
<visual>
<!-- 沿着自己几何中心的偏移和旋转 -->
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<!-- 几何形状 -->
<geometry>
<!-- 圆柱体 -->
<cylinder radius="${radius}" length="${length}"/>
</geometry>
<!-- 材质和颜色 -->
<material name="white">
<color rgba="1.0 1.0 1.0 0.5" />
</material>
</visual>
</link>
</xacro:macro>
<xacro:macro name="imu_link" params="imu_name xyz">
<!-- 机器人的 IMU ，惯性测量单元 -->
<link name="${imu_name}_link">
<!-- 部件的外观描述 -->
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- 正方体 -->
<box size="0.02 0.02 0.02" />
</geometry>
<!-- 材质和颜色 -->
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 机器人关节，用来组合机器人部件 -->
<joint name="${imu_name}_joint" type="fixed">
<!-- 父部件 -->
<parent link="base_link"/>
<!-- 子部件固定到父部件上 -->
<child link="${imu_name}_link"/>
<!-- 固定的位置 -->
<origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
</joint>
</xacro:macro>
<xacro:base_link length="0.12" radius="0.1"/>
<xacro:imu_link imu_name="imu_up" xyz="0.0 0.0 0.03"/>
<xacro:imu_link imu_name="imu_down" xyz="0.0 0.0 -0.03"/>
</robot>
|
通过 xacro first_robot.urdf 就可以将上面的文件内容生成机器人模型 xml
使用 rviz 可视化 urdf
安装 ros-jazzy-robot-state-publisher 和 ros-jazzy-joint-state-publisher 两个功能包，这两个功能包可以解析 urdf 文件，自动发布对应的 TF 关系
1
| sudo apt install ros-jazzy-joint-state-publisher ros-jazzy-robot-state-publisher
|
写 launch 文件，同时加载各个节点
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
| # -*- coding: utf-8 -*-
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument
def generate_launch_description():
share_dir = FindPackageShare("robot_describes")
default_model_path = PathJoinSubstitution([share_dir, "urdf", "first_robot.urdf"])
default_config_path =PathJoinSubstitution([share_dir, "config", "display_robot.rviz"])
model_path = LaunchConfiguration("model", default=default_model_path)
config_path = LaunchConfiguration("config", default=default_config_path)
urdf = Command(["xacro ", model_path])
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
output="screen"
)
joint_state_publisher = Node(
package="joint_state_publisher",
executable="joint_state_publisher"
)
rviz2 = Node(
package="rviz2",
executable="rviz2",
arguments=["-d", config_path]
)
return LaunchDescription([
DeclareLaunchArgument("model", default_value=default_model_path, description="模型路径"),
DeclareLaunchArgument("config", default_value=default_config_path, description="配置路径"),
robot_state_publisher,
joint_state_publisher,
rviz2])
|
display_robot.rviz 为 rviz2 工具的配置文件，在 rviz2 工具中配置后保存即可
使用 ros2 launch robot_describes display_robot.py 启动就可以在 rviz2 中看到对应的机器人模型
增加物理属性
碰撞属性
碰撞属性使用 collision 标签添加，和 visual 类似
1
2
3
4
5
6
7
8
9
10
11
| <collision>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<geometry>
<!-- 正方体 -->
<box size="0.02 0.02 0.02" />
</geometry>
<!-- 材质和颜色 -->
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</collision>
|
质量和惯性
使用 inertial 添加质量和惯性，质量用 mass 表示，单位为 kg ，惯性用 inertia ，使用惯性矩阵
常见三种形状的质量和惯性
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
| <?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="box_inertia" params="m w h d">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertia" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0" ixz="0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0" izz="${(m/2) * (r*r)}" />
</inertial>
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}" />
</inertial>
</xacro:macro>
</robot>
|
Gazebo 仿真
ROS2 启动 Gazebo 并加载模型
可以在 launch 文件中启动 gazebo ，同时将之前创建的机器人模型创建出来
启动 gazebo 仿真，加载世界模型
1
2
3
4
5
6
7
8
9
| default_world_path = PathJoinSubstitution([share_dir, "world", "xbot.sdf"])
gz_launch_path = PathJoinSubstitution([FindPackageShare("ros_gz_sim"), 'launch', 'gz_sim.launch.py'])
gz_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(gz_launch_path),
launch_arguments={
'gz_args': [default_world_path],
'on_exit_shutdown': 'True'
}.items(),
)
|
在世界中生成机器人
同样启动 robot_state_publisher 解析 URDF 文件并发布，同时计算各个关节的 TF 关系
1
2
3
4
5
6
| robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
output="screen"
)
|
通过 ros_gz_sim 包创建机器人，机器人模型来源 robot_description
1
2
3
4
5
6
7
8
| spawn_entity = Node(
package="ros_gz_sim",
executable="create",
arguments=[
"-name", "xbot",
"-topic", "robot_description",
]
)
|
启动 bridge
birdge 的作用是将 gz 和 ros2 的话题之间做一个转换，比如将 gz 发布的 tf 转发到 ros2 下，支撑的转换类型：https://github.com/gazebosim/ros_gz/tree/ros2/ros_gz_bridge
启动 bridge ，配置文件如下：
- ros_topic_name: "clock"
gz_topic_name: "/clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
direction: "GZ_TO_ROS"
- gz_topic_name: "/cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "gz.msgs.Twist"
- ros_topic_name: "tf"
gz_topic_name: "/tf"
ros_type_name: "tf2_msgs/msg/TFMessage"
gz_type_name: "gz.msgs.Pose_V"
direction: "GZ_TO_ROS"
启动代码：
1
2
3
| from ros_gz_bridge.actions import RosGzBridge
bridge_config_path = PathJoinSubstitution([share_dir, "config", "bridge.yaml"])
bridge = RosGzBridge(bridge_name="xbot_bridge", config_file=bridge_config_path)
|
完整的 launch 代码：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
| # -*- coding: utf-8 -*-
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ros_gz_bridge.actions import RosGzBridge
def generate_launch_description():
share_dir = FindPackageShare("robot_describes")
default_model_path = PathJoinSubstitution([share_dir, "urdf", "xbot", "xbot.urdf.xacro"])
bridge_config_path = PathJoinSubstitution([share_dir, "config", "bridge.yaml"])
default_world_path = PathJoinSubstitution([share_dir, "world", "xbot.sdf"])
rviz_config_path =PathJoinSubstitution([share_dir, "config", "display_robot.rviz"])
model_path = LaunchConfiguration("model", default=default_model_path)
urdf = Command(["xacro ", model_path])
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[{"robot_description": ParameterValue(urdf, value_type=str)}],
output="screen"
)
gz_launch_path = PathJoinSubstitution([FindPackageShare("ros_gz_sim"), 'launch', 'gz_sim.launch.py'])
gz_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(gz_launch_path),
launch_arguments={
'gz_args': [default_world_path],
'on_exit_shutdown': 'True'
}.items(),
)
bridge = RosGzBridge(bridge_name="xbot_bridge", config_file=bridge_config_path)
spawn_entity = Node(
package="ros_gz_sim",
executable="create",
arguments=[
"-name", "xbot",
"-topic", "robot_description"
]
)
rviz2 = Node(
package="rviz2",
executable="rviz2",
arguments=["-d", rviz_config_path]
)
return LaunchDescription([
DeclareLaunchArgument("model", default_value=default_model_path, description="模型路径"),
robot_state_publisher,
gz_launch,
bridge,
spawn_entity,
rviz2
])
|
两轮差速传感器仿真
gazebo 通过插件形式提供各种传感器仿真，可以在 urdf 中使用 gazebo 扩展标签描述插件
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
| <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="gazebo_control_plugin">
<gazebo>
<!-- 两轮差速插件 -->
<plugin name='gz::sim::systems::DiffDrive' filename='gz-sim-diff-drive-system'>
<!-- 左右轮子 -->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<!-- 轮子之间的距离 -->
<wheel_separation>0.2</wheel_separation>
<!-- 轮子的半径 -->
<wheel_radius>0.032</wheel_radius>
<!-- 里程计 -->
<frame_id>odom</frame_id>
<!-- tf 转换， odom 转换到 base_footprint -->
<child_frame_id>base_footprint</child_frame_id>
<!-- 接受控制的 topic -->
<topic>/cmd_vel</topic>
<!-- 发布里程的 topic -->
<odom_topic>/odometry</odom_topic>
<!-- tf 转换的 topic -->
<tf_topic>/tf</tf_topic>
</plugin>
<!-- 发布关节状态 -->
<plugin name='gz::sim::systems::JointStatePublisher' filename='gz-sim-joint-state-publisher-system'>
<topic>/joint_states</topic>
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
</plugin>
</gazebo>
</xacro:macro>
</robot>
|
然后再 xbot 中引用
1
2
| <xacro:include filename="$(find robot_describes)/urdf/xbot/plugins/gazebo_control_plugin.xacro"/>
<xacro:gazebo_control_plugin/>
|
上面的插件会在 gazebo 中发布 /odometry ， /joint_states 话题，因此需要在 bridge 中配置，将对应内容转换到 ros2 中
- ros_topic_name: "odometry"
gz_topic_name: "/odometry"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.Odometry"
direction: "GZ_TO_ROS"
- ros_topic_name: "joint_states"
gz_topic_name: "/joint_states"
ros_type_name: "sensor_msgs/msg/JointState"
gz_type_name: "gz.msgs.Model"
direction: "GZ_TO_ROS"
激光雷达传感器仿真
同样增加激光雷达插件描述
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
| <gazebo>
<plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
</gazebo>
<gazebo reference="laser_link">
<sensor name="gpu_lidar" type="gpu_lidar">
<!-- 发布数据的话题名称 -->
<topic>scan</topic>
<!-- 发布频率 1 HZ -->
<update_rate>1</update_rate>
<!-- 发布坐标数据用的 frame_id -->
<gz_frame_id>laser_link</gz_frame_id>
<ray>
<!-- 设置扫描范围 -->
<scan>
<horizontal>
<!-- 一圈采样点 -->
<samples>360</samples>
<!-- 分辨率 -->
<resolution>1.0</resolution>
<!-- 最小角度 -->
<min_angle>0.0</min_angle>
<!-- 最大角度 -->
<max_angle>6.28</max_angle>
</horizontal>
</scan>
<!-- 扫描距离 -->
<range>
<!-- 最短距离 -->
<min>0.12</min>
<!-- 最大距离 -->
<max>8.0</max>
<!-- 分辨率 -->
<resolution>0.015</resolution>
</range>
</ray>
<!-- 总是扫描 -->
<always_on>1</always_on>
<!-- 可视化打开 -->
<visualize>true</visualize>
</sensor>
</gazebo>
|
在 bridge 中配置话题转换
- ros_topic_name: "lidar_scan"
gz_topic_name: "scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: "GZ_TO_ROS"
IMU 仿真
添加插件配置
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
| <gazebo>
<plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu">
</plugin>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<!-- 发布话题名称 -->
<topic>imu</topic>
<always_on>1</always_on>
<!-- 更新频率 -->
<update_rate>1</update_rate>
<!-- 可视化 -->
<visualize>true</visualize>
<!-- 六轴噪声设置 -->
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
|
bridge 中增加 topic 转换
- ros_topic_name: "imu"
gz_topic_name: "imu"
ros_type_name: "sensor_msgs/msg/Imu"
gz_type_name: "gz.msgs.IMU"
direction: "GZ_TO_ROS"
深度相机仿真
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
| <!-- 深度相机 -->
<gazebo reference="camera_link">
<sensor name="grbd_camera" type="rgbd_camera">
<topic>camera</topic>
<update_rate>1</update_rate>
<always_on>1</always_on>
<visualize>true</visualize>
<gz_frame_id>camera_link</gz_frame_id>
<camera name="head">
<optical_frame_id>camera_link</optical_frame_id>
<!-- 水平视野角度 -->
<horizontal_fov>1.3</horizontal_fov>
<image>
<!-- 图像宽度 -->
<width>320</width>
<!-- 图像高度 -->
<height>240</height>
</image>
<clip>
<!-- 近裁剪平面 -->
<near>0.02</near>
<!-- 远裁剪平面 -->
<far>12</far>
</clip>
</camera>
</sensor>
</gazebo>
|
bridge 中增加对应的转换
- ros_topic_name: "/camera/depth_image"
gz_topic_name: "/camera/depth_image"
ros_type_name: "sensor_msgs/msg/Image"
gz_type_name: "gz.msgs.Image"
direction: "GZ_TO_ROS"
- ros_topic_name: "/camera/image"
gz_topic_name: "/camera/image"
ros_type_name: "sensor_msgs/msg/Image"
gz_type_name: "gz.msgs.Image"
direction: "GZ_TO_ROS"
- ros_topic_name: "/camera/points"
gz_topic_name: "/camera/points"
ros_type_name: "sensor_msgs/msg/PointCloud2"
gz_type_name: "gz.msgs.PointCloudPacked"
direction: "GZ_TO_ROS"