【从零开始的ROS四轴机械臂控制(三)】
-
- 五、在gazebo中添加摄像头
-
- 1.修改arm1.gazebo.xacro文件
- 2.修改arm1.urdf.xacro文件
- 3.查看摄像头图像
- 六、为模型添加夹爪(Gripper)
-
- 1.通过solidworks建立模型
- 2.将夹爪添加进gazebo
-
- (1)模型导入
- (2)更改urdf文件夹
- 3.gazebo模型抖动解决办法
-
-
- ①pid参数设置
- ②旋转惯量参数矩阵
-
- 4.制作物块sdf模型
五、在gazebo中添加摄像头
在完成simple_mover之后,回顾arm0.1版本的目标,是对不同颜色的方块进行分类并在Gazebo中模拟出来。 要实现不同颜色方块的分类,就需要用到摄像头了。所以接下来的任务是为模型添加一个摄像头,以供以后图像处理使用。若要实现摄像头功能,则需要添加相应的插件。可以参考:http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros。 本部分完成效果如下:
我们需要更改urdf文件夹下arm1.urdf.xacro、arm1.gazebo.xacro两个文件。这个作为Robot arm 0.1.2 版本已经上传github。
1.修改arm1.gazebo.xacro文件
首先我们需要向arm1.gazebo.xacro文件中添加camera插件:
<!--link_4-->
<?xml version="1.0"?>
<robot>
...(部分省略)
<gazebo reference="link4">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<!--camera_link-->
<gazebo reference="camera_link">
<material>Gazebo/Grey</material>
</gazebo>
<!-- camera-->
<gazebo reference="camera_link">
<sensor type="camera" name="rgb_camera">
<update_rate>30.0</update_rate>
<camera name="rgb_camera">
<horizontal_fov>1.4</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>rgb_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.0</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>
让我们讨论一下此插件的一些属性…
<gazebo reference="camera_link">
链接名称“ camera_link”必须与我们添加到Xacro URDF的链接的名称匹配。
<code class="prism language-xml has-numbering"> <span class="token tag"><span class="token punctuation"><</span>sensor <span class="token attr-name">type</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>camera<span class="token punctuation">"</span></span> <span class="token attr-name">name</span><span class="token attr-value"><span class="token punctuation">=</span><span class="token punctuation">"</span>rgb_camera<span class="token punctuation">"</span></span><span class="token punctuation">></span></span> </code>
传感器名称“ rgb_camera
”必须保证相对于其他传感器名称唯一。除了可以访问的Gazebo插件内,此名称在很多地方都没有使用
<code class="prism language-xml has-numbering"> <span class="token tag"><span class="token punctuation"><</span>update_rate<span class="token punctuation">></span></span>30.0<span class="token tag"><span class="token punctuation"></</span>update_rate<span class="token punctuation">></span></span> </code>
gazebo内每秒拍摄一次新相机图像的次数。这是传感器在仿真过程中将尝试的最大更新速率,但是如果物理仿真的运行速度快于传感器生成的速度,则它可能会落后于该目标速率。
<horizontal_fov>1.4</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
填写这些值以匹配物理相机硬件上制造商的规格。要注意的一件事是,假定像素为正方形。 此外,远近剪辑是特定于模拟的参数,它给出摄像机在模拟中可以看到物体的距离的上限和下限。这是在相机的视光框中指定的。
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
这是实际文件作为共享对象链接到的地方。gazebo_ros/gazebo_ros_camera.cpp
<cameraName>rgb_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
在这里,我们为图像主题和照相机信息主题定义了照相机将发布到的rostopic。 对于arm1,我们用以下命令来订阅:
/arm1/rgb_camera/image_raw
/arm1/rgb_camera/camera_info
<frameName>camera_link</frameName>
图像的坐标系被发布在 tf树 下。
2.修改arm1.urdf.xacro文件
若将摄像机添加在urdf文件中,则需在arm1.urdf.xacro文件中添加以下内容:
<!--camera_link-->
<link name="camera_link">
<collision>
<origin xyz="1 0 0" rpy="0 0 0"/>
<geometry>
<!--mesh filename="package://arm1/meshes/camera.dae"/-->
<sphere radius="0.005"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!--mesh filename="package://arm1/meshes/camera.dae"/-->
<sphere radius="0.005"/>
</geometry>
<material name="red">
<color rgba="1.0 0 0 1.0"/>
</material>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0.05 0" rpy="0 1.57 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!--camera_joint-->
<joint name="camera_joint" type="fixed">
<axis xyz="0 0 0" />
<origin xyz="-0.042 -0.035 -0.01" rpy="-1.57 -3.1415 0"/>
<parent link="link4"/>
<child link="camera_link"/>
</joint>
其中,
<geometry>
<!--mesh filename="package://arm1/meshes/camera.dae"/-->
<sphere radius="0.005"/>
</geometry>
我们定义了一个半径为0.005m的球体当作摄像头,所以在标签中写作<sphere radius="0.005"/>
。除此之外,也可以使用模型<mesh filename="package://arm1/meshes/camera.dae"/>
作为摄像头,这里因为摄像头尺寸问题,所以选择使用球体作为摄像头的模型。
将更改的文件保存后,我们就可以运行gazebo来仿真了。 新建一个终端,输入
$ killall gzserver
$ source ~/catkin_ws/devel/setup.bash
$ roslaunch arm1 gazebo.launch
其中killall gzserver
作用为关闭所有gazebo服务,可能是因为虚拟机的问题还是别的,gazebo总是出问题,所以最好每次运行新的终端的时候输入此命令来防止gazebo加载出错。 若无意外,gazebo应该打开如下图,可以看到camera已经成功加载出来了。
3.查看摄像头图像
要查看摄像机图像流,可以使用命令rqt_image_view
(在此处了解有关rqt和相关工具的更多信息)
<code class="prism language-cmd has-numbering">$ rqt_image_view /rgb_camera/image_raw </code>
打开如下视图
为了方便测试摄像头,可以选择gazebo左上角的insert标签,插入某些模型,其效果如下
六、为模型添加夹爪(Gripper)
为了实现夹取操作,需要为我们的模型添加夹爪。 我还是选择在solidworks 中来绘制模型。导出stl文件和更改urdf文件。
1.通过solidworks建立模型
首先在solidworks中建立模型,因为只是在gazebo中模拟,我就画的比较简陋了一点…
具体方法还是可以参照【从零开始的ROS四轴机械臂控制】(三),还是可以选择导出urdf文件和具体模型。 夹爪由三个link组成,分别是gripper_base_link(后面称之为gripper_link),right_finger_link(后面称之为right_link)和left_finger_link(后面称之为left_link),还有两个joint,称之为right_joint 和 left_joint。在之后导出urdf文件后,会导出三个link的stl文件。 因为我们在~/catkin_ws/src目录下已经有arm1的包了,所以现在需要做的是把夹爪添加到我们的模型中。个人感觉,其实不如一开始就把所有模型都建好,因为后期导入新的模型会出现很多的bug。
2.将夹爪添加进gazebo
(1)模型导入
最简单的一步,就是把新导出的三个stl文件拷贝进meshes文件夹
(2)更改urdf文件夹
拷贝完模型,接下来就是更改urdf文件,和添加摄像头类似,添加了各个joints,links还有controllers,并对相应文件进行了添加修改,更多的这里不再赘述。完全可以参考上面如何添加摄像头。 部分添加的urdf文件如下所示:
...
<!--gripper links-->
<!--gripper base-->
<link
name="gripper_link">
<inertial>
<origin
xyz="-0.0066122 0.0073345 0.035"
rpy="0 0 0" />
<mass
value="0.0035" />
<inertia
ixx="1.2396E-07"
ixy="-1.3634E-23"
ixz="1.9573E-24"
iyy="3.6458E-07"
iyz="5.8326E-24"
izz="4.7396E-07" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/gripper_Link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/gripper_Link.STL" />
</geometry>
</collision>
</link>
<joint name="joint_gripper"
type="fixed">
<origin
xyz="-0.045 -0.035 -0.01"
rpy="0 1.57 -2.45" />
<parent
link="link4" />
<child
link="gripper_link" />
</joint>
<!--gripper_right link and joint-->
<link
name="right_link">
<inertial>
<origin
xyz="-0.0085551 0.004 0.024348"
rpy="0 0 0" />
<mass
value="0.0013847" />
<inertia
ixx="1.28810723367291E-06"
ixy="-4.80571958593516E-08"
ixz="-1.80536823271041E-09"
iyy="1.3067705064679E-06"
iyz="-1.51983137928678E-09"
izz="5.58044990324735E-07" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/right_Link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/right_Link.STL" />
</geometry>
</collision>
</link>
<joint
name="right_joint"
type="revolute">
<origin
xyz="0.0125 -0.004 0"
rpy="-1.5708 0 0" />
<parent
link="gripper_link" />
<child
link="right_link" />
<axis
xyz="0 0 1" />
<limit
lower="-0.4"
upper="0.4"
effort="1.0"
velocity="0.5" />
</joint>
<!--gripper_left link and joint-->
<link
name="left_link">
<inertial>
<origin
xyz="-0.0085551 -0.004 0.024348"
rpy="0 0 0" />
<mass
value="0.0013847" />
<inertia
ixx="1.28810723367291E-06"
ixy="-4.80571958593516E-08"
ixz="-1.80536823271041E-09"
iyy="1.3067705064679E-06"
iyz="-1.51983137928678E-09"
izz="5.58044990324735E-07" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/left_Link.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://arm1/meshes/left_Link.STL" />
</geometry>
</collision>
</link>
<joint
name="left_joint"
type="revolute">
<origin
xyz="-0.0125 -0.004 0"
rpy="-1.5708 0 0" />
<parent
link="gripper_link" />
<child
link="left_link" />
<axis
xyz="0 0 -1" />
<limit
lower="-0.4"
upper="0.4"
effort="1.0"
velocity="0.5" />
</joint>
<!--camera_link-->
<link name="camera_link">
...
之所以不建议后来添加模型是因为joint位置问题,如果后来添加模型,需要对<joint>
的<origin>
标签经行仔细修改。而如果直接使用solidworks就不用这么麻烦,而且还可以避免一定的模型抖动问题。 若添加无问题则如下图所示,
添加进几个模型,执行命令:
$ rqt_image_view /rgb_camera/image_raw
<code class="has-numbering"></code>
可以达到如下图所示的效果
3.gazebo模型抖动解决办法
我总共遇到了两种情况的模型抖动,一是为pid参数设置,二是为旋转惯量参数矩阵设置。这两种情况都会造成模型的抖动,其它的情况没有遇到过,等以后有机会遇到会再总结一次的。
①pid参数设置
在controllers.yaml文件中,记录着各个控制器的pid参数
arm1:
#list of controllers
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 1, i: 0.001, d: 0.002}
joint2_position_controller:
type: effort_controllers/JointPositionController
joint: joint2
pid: {p: 1, i: 0.001, d: 0.002}
...
pid参数的值过大会造成震荡等问题,过小就会控制效果差。所以要选择适合自己模型的pid参数,才能达到最好的效果。那什么是最好的参数选择呢?..这就是门很深的学问了,可以自行百度,或者自己多次尝试更改…
②旋转惯量参数矩阵
在arm1.urdf.xacro文件中,在link的<inertial>
标签下,都有类似于如下的内容
<inertia
ixx="7.57454654701591E-08"
ixy="2.28779573583169E-08"
ixz="8.70361108935048E-14"
iyy="1.66799537620888E-08"
iyz="3.19929708005816E-14"
izz="8.54396424256289E-08" />
3×3转动惯量矩阵由惯量元素指定。因为它是对称的,所以它只能由6个元素来表示
还是那句话,选择合适的转动惯量矩阵的参数会减少模型的震动。 最终效果如下所示:
本部分代码已上传github,接下来的博客便是关于添加相关节点的了。
4.制作物块sdf模型
我们需要几个物块作为夹取目标,所以一个比较好的方法就是直接使用gazebo建立sdf模型。我准备制作两种颜色20 m m 3 20mm^320mm3的立方体作为抓取目标。 在打开的gazebo中选择Edit–>Model Editor
插入一个立方体
更改相应link,Visual,Collision
选择ok进行确认。保存模型,将其插入到我们的机械臂世界中。效果如下图所示。