目前的空余时间主要都在研究ROS2,最终目的是控制自己用舵机组装的机械手。
由于种种原因,先控制Gazebo的自定义机械手。
先看看目前的成果
左侧是rviz2中的moveit组件的机械手,右侧是gazebo中的机械手。在moveit中进行路径规划并执行后,右侧gazebo中的机械手也就执行相应的动作。
1.软件的安装
1.1.Ubuntu 22.04
据说win10下也可以装ROS2,但是十分折腾,还是在Ubuntu下安装方便一些,Ubuntu22.04才有humble版本的ros2,Ubuntu20.04没有humble。
1.2.ROS2
按照鱼香ros的教程,在命令行中执行鱼香ROS的一键安装代码
wget http://fishros.com/install -O fishros && . fishros
然后选择安装ros,选择humble版本。
1.3.Gazebo
参考教程
只要执行两行就行
sudo apt install gazebo11
sudo apt install ros-humble-gazebo-*
1.4.MoveIt2
参考教程
MoveIt2可以选择自己编译源码安装,或者直接从二进制安装。
个人建议直接二进制安装,可以省很多事。
sudo apt install ros-humble-moveit
1.5.moveit-setup-assistant
这是一个配套moveit的配置助手,有了它就可以方便地进行很多初始化的工作。
sudo apt install ros-humble-moveit-setup-assistant
2.机械手urdf文件的编写
我们用urdf文件来描述我们的机械手的外观以及物理性能。这里为了简便,就只用了基本的圆柱、立方体了。追求美观的朋友,还可以用dae文件来描述机械手的外形。
文件six_arm.urdf
<?xml version="1.0"?>
<robot name="six_arm">
<link name="base_link">
<visual>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="blue">
<color rgba="0 0 1.0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<link name="link1">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="green">
<color rgba="0 0.8 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="base_link"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
</joint>
<link name="link2">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="red">
<color rgba="0.8 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.1"/>
</joint>
<link name="link3">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="yellow">
<color rgba="0.8 0.8 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint3" type="continuous">
<parent link="link2"/>
<child link="link3"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.1"/>
</joint>
<link name="link4">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="green">
<color rgba="0 0.8 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint4" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<axis xyz="0 1 0"/>
<origin xyz="0 0 0.1"/>
</joint>
<link name="link5">
<visual>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<material name="purple">
<color rgba="0.8 0 0.8 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.1" radius="0.03"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint5" type="continuous">
<parent link="link4"/>
<child link="link5"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.1"/>
</joint>
<link name="link6">
<visual>
<geometry>
<box size="0.1 0.1 0.2"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<material name="pink">
<color rgba="0.8 0.4 0.8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.1 0.1 0.2"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="joint6" type="continuous">
<parent link="link5"/>
<child link="link6"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.1"/>
</joint>
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
<gravity>true</gravity>
<selfCollide>false</selfCollide>
</gazebo>
<gazebo reference="link1">
<material>Gazebo/Gray</material>
<selfCollide>false</selfCollide>
</gazebo>
<gazebo reference="link2">
<material>Gazebo/Red</material>
<selfCollide>false</selfCollide>
</gazebo>
<gazebo reference="link3">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="link4">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="link5">
<material>Gazebo/Yellow</material>
</gazebo>
<gazebo reference="link6">
<material>Gazebo/Orange</material>
</gazebo>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint3">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint4">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint5">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint6">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>/home/yong/Desktop/myRobot/src/mybot/config/ros2_controllers.yaml</parameters>
<robot_param_node>robot_state_publisher</robot_param_node>
</plugin>
</gazebo>
</robot>
2.1.机械手的外型
可以在vscode中打开这个文件然后用插件就可以看到这个机械手的外形,关于urdf语法的详细介绍,可以看这里。
2.2.机械手link的inertial的设置
我这里偷懒,随便设置的。真正设置的话,有公式,自己可以参考一下。
urdf里面的link必须要有旋转惯量矩阵‘intertial’的,否则在gazebo里面导入模型urdf时,会报下面的错。
[gazebo-1] [Err] [Model.cc:123] Error Code 23 Msg: FrameAttachedToGraph error, Non-LINK vertex with name [model] is disconnected; it should have 1 outgoing edge in MODEL attached_to graph.
2.3.ros2_control插件
该插件是在gazbo导入该模型文件时,创建与Ros2交互的接口。
上面的ros2_controllers.yaml文件是在下一步创建出来的,先不用管。
同时,ros2_control这个节点下的内容也是要和ros2_controllers.yaml对应的,也可以先不管。
这个节点在开始阶段先注释掉,否则会与通过moveit_setup_assistant创建的一个fake_systemde ros2_control冲突。
3.机械手与MoveIt的关联
通过前面的操作,我们拥有了一个描述机械手的文件 six_arm.urdf,接下来我们利用该文件创建一个可以利用MoveIt进行路径规划的“工程”。
3.1.建立一个文件夹myRobot,然后再在此文件夹中建立一个src文件夹
3.2.然后进入src文件夹路径,创建包
ros2 pkg create mybot_description --build-type ament_python
3.3.在 src/mybot_description文件夹下,创建urdf文件夹,然后把six_arm.urdf放进去,如下图所示。
3.4.返回myRobot目录,然后编译一下
colcon build
3.5.source一下
source的目的是为了把我们创建的 mybot_description项目暴露给控制台,然后让后续的moveit_setup_assistant可以找到对应的urdf文件
source install/setup.bash
3.6.利用moveit_setup_assistant创建项目
参考教程,启动moveit_setup_assistant
ros2 run moveit_setup_assistant moveit_setup_assistant
记得选择src里面的urdf,然后后面的就按照教程来干了。
注意:group name设置my_group
一些关键的设置看下列图
在最后一步保存时,在我们前面创建的src文件夹里面,创建一个mybot文件夹
然后在助手里选择该路径,点击generat package
3.7.编译试运行
此时,退出助手,并执行编译;
编译完成后,重新source一下。
colcon build
source install/setup.bash
记得把前面的urdf文件里面的这个路径改成你自己的。
这段注释一下,否则会有冲突。到后面就可以取消注释,但是目前要注释掉。
每次修改完文件后,记得都要 colcon build一下。
试着执行
ros2 launch mybot demo.launch.py
会报错
这是因为在编译时没有把urdf文件拷贝到install目录中,因此我们需要修改一下 src/mybot_description/setup.py
修改三处地方,将其修改成类似下面这样
修改完之后就可以自动在编译时复制到install目录了,具体原理可以自己按照代码推测一下
from setuptools import setup
from glob import glob
import os
package_name = 'mybot_description'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'urdf'), glob('urdf/**')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='yong',
maintainer_email='yong@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
],
},
)
colcon build一下。
然后启动
ros2 launch mybot demo.launch.py
然后就可以愉快地玩耍了。
假如报其他错误请查看此处。
4.机械手与Gazebo的关联
4.1.在 src/mybot/launch 路径下,新建一个文件: gazebo.launch.py
其内容如下
import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess, RegisterEventHandler
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
def generate_launch_description():
robot_name_in_model = 'six_arm'
package_name = 'mybot_description'
urdf_name = "six_arm.urdf"
pkg_share = FindPackageShare(package=package_name).find(package_name)
urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')
start_gazebo_cmd = ExecuteProcess(
cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
output='screen')
spawn_entity_cmd = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-entity', robot_name_in_model, '-file', urdf_model_path ], output='screen')
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path],
parameters=[{'use_sim_time': True}],
output='screen'
)
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_state_broadcaster'],
output='screen'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'my_group_controller'],
output='screen'
)
close_evt1 = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity_cmd,
on_exit=[load_joint_state_controller],
)
)
close_evt2 = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_joint_trajectory_controller],
)
)
ld = LaunchDescription()
ld.add_entity(close_evt1)
ld.add_entity(close_evt2)
ld.add_action(start_gazebo_cmd)
ld.add_action(node_robot_state_publisher)
ld.add_action(spawn_entity_cmd)
return ld
4.2.将 src/mybot_description/urdf/six_arm.urdf 的内容修改一下
将这一段取消注释。
4.3.编译、运行
每次修改完文件后,记得都要 colcon build一下。
colcon build一下。
然后启动
ros2 launch mybot gazebo.launch.py
然后就可以愉快地玩耍了。
4.4. action send_goal 测试
目前运行的这这个gazebo例子已经开放了一个action接口,通过这个接口,我们就可以直接发送一个路径让其执行。
我们可以手写一个轨迹,然后让其执行
新建一个my_send_goal.sh文件,然后通过 chmod +x 使其具有执行权限
文件内容如下:
ros2 action send_goal /my_group_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{
trajectory: {
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6],
points: [
{ positions: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], time_from_start: { sec: 0, nanosec: 500 } },
{ positions: [0.2, 0.5, 0.2 ,0.2, 0.2, 0.2], time_from_start: { sec: 5, nanosec: 500 } },
{ positions: [0.3, 0.3, 0.7, -0.5, 0.3, 0.3], time_from_start: { sec: 7, nanosec: 500 } },
{ positions: [0.4, 0.4, 0.9, 0.4, 0.4, 0.4], time_from_start: { sec: 8, nanosec: 500 } }
]
}
}"
然后在控台里面执行
./my_send_goal.sh
接下来可以看到gazebo中的机械手按照我们设定的轨迹进行运动了。
可以尝试改变一下my_send_goal.sh 的内容,然后推测一下各个参数的意义。
5.MoveIt与Gazebo的关联
moveit提供了路径规划接口,gazebo提供了执行接口,因此只需要将他们连起来就实现了路径规划仿真了。
5.1.原理分析
在前面通过moveit_setup_assistant创建的包中,之所以可以进行路径规划并执行,是因为他既启动了moveit的组件,也启动了一个FakeSystem节点,该节点代替了实际的机械手反馈moveit的路径规划(提供action给moveit连接)。
从下图可以看出,当我们执行demo时,有好几个节点启动了。
但是假如我们需要将FakeSystem节点替换成gazebo的节点,就可以通过只启动需要的几个节点,然后不启动FakeSystem就行。
5.2.launch文件编写
按照5.1的分析,我们可以在src/mybot/launch下新建一个文件my_moveit_rviz.launch.py,
文件内容为
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_moveit_rviz_launch
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
)
from moveit_configs_utils.launch_utils import (
add_debuggable_node,
DeclareBooleanLaunchArg,
)
from launch.substitutions import LaunchConfiguration
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("six_arm", package_name="mybot_description").to_moveit_configs()
ld = LaunchDescription()
my_generate_move_group_launch(ld, moveit_config)
my_generate_moveit_rviz_launch(ld, moveit_config)
return ld
def my_generate_move_group_launch(ld, moveit_config):
ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
ld.add_action(
DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True)
)
ld.add_action(
DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
)
ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False))
should_publish = LaunchConfiguration("publish_monitored_planning_scene")
move_group_configuration = {
"publish_robot_description_semantic": True,
"allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"),
"capabilities": ParameterValue(
LaunchConfiguration("capabilities"), value_type=str
),
"disable_capabilities": ParameterValue(
LaunchConfiguration("disable_capabilities"), value_type=str
),
"publish_planning_scene": should_publish,
"publish_geometry_updates": should_publish,
"publish_state_updates": should_publish,
"publish_transforms_updates": should_publish,
"monitor_dynamics": False,
}
move_group_params = [
moveit_config.to_dict(),
move_group_configuration,
]
move_group_params.append({"use_sim_time": True})
add_debuggable_node(
ld,
package="moveit_ros_move_group",
executable="move_group",
commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"),
output="screen",
parameters=move_group_params,
extra_debug_args=["--debug"],
additional_env={"DISPLAY": ":0"},
)
return ld
def my_generate_moveit_rviz_launch(ld, moveit_config):
"""Launch file for rviz"""
ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
ld.add_action(
DeclareLaunchArgument(
"rviz_config",
default_value=str(moveit_config.package_path / "config/moveit.rviz"),
)
)
rviz_parameters = [
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
]
rviz_parameters.append({"use_sim_time": True})
add_debuggable_node(
ld,
package="rviz2",
executable="rviz2",
output="log",
respawn=False,
arguments=["-d", LaunchConfiguration("rviz_config")],
parameters=rviz_parameters,
)
return ld
在文件中,我们主要启动了两个节点:move_group、rviz。
这里面这还涉及到一个use_sim_time的问题,因此才这样重写。参考这里
然后colcon build一下。
5.2.执行测试
分别打开两个控制台,然后分别执行以下命令
ros2 launch mybot gazebo.launch.py
ros2 launch mybot my_moveit_rviz.launch.py
注意是要先运行gazebo(ros2 launch mybot gazebo.launch.py),然后再运行moveit(ros2 launch mybot my_moveit_rviz.launch.py )
在moveit里面规划路径、然后执行。
在gazebo就可以看到模型动了起来。
6.总结
目前只是通过大致的操作完成了moveit控制gazebo里面的机械手,是偏操作方面的,原理可以粗略地理解成action的通讯。至于更加详细以及深入的教程,就等各位大佬来完成了。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)