利用ros2 control获取gazebo中六维力传感器数据,使用force_torque_broadcaster
因为目前需要在仿真环境中模拟机械臂末端力传感器的数据,但是目前基于ros2的资料又比较少,笔者在经历数次尝试之后终于总结出一个不错的方案,供大家参考。
关于ROS2 CONTROL
首先让我们来看一下ros2 control的框架,最上面的一层controllers会计算系统的输入输出,比如说最熟悉的joint_trajectory_controller便是输出轨迹,最后通过硬件层来执行计算好的轨迹,这样就省下了我们很多的工作。笔者这里只讲force_torque_broadcaster,他的作用是将传感器的数据广播出去。
一般步骤
首先是对URDF文件进行编写
笔者这里假设你已经有了一个完整的URDF文件,并且可以在RVIZ2中显示出来,如果不知道步骤的话可以参考鱼香ros的教程:《动手学ROS2进阶篇》8.2RVIZ2可视化移动机器人模型
笔者使用的是sw2urdf插件将solidworks里机械臂的模型转换为了urdf文件,并且进行重新的编写和改进,转换成xacro文件,其中有个问题需要注意,对于mesh文件(dae、stl),gazebo那边识别不了 package://这种ros系统的路径查找方式,只能用 $(find xxx)这种方式找到绝对路径并替换。但是,rviz这边又不认这种绝对路径,解决办法可以看这里ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真
如果出现零件乱飞或者抽搐的情况,多半是因为惯性参数(inertial)没有编写到位,因为sw2urdf是有bug的,惯性参数会导出错误,这时就需要我们重新赋予零件惯性参数,笔者这里使用meshlab重新计算惯性参数,如果不会使用可以看这个V-rep中机械臂惯性参数的获取方法
这样下来你应该就可以获得一份可以在gazebo中显示的urdf文件了,下面要做的便是笔者的重点
如何添加ros2 control的tag?
如果要使用ros2 control首先要在urdf文件中声明你需要的command interface和state interface,添加形式如下所式,在你的urdf文件中<robot的tag里添加ros2 control的代码块,因为我们要使用的是gazebo,所以这里需要引入gazebo的插件。在harware里joint自不不必多说,是关节的控制,我们主要需要的是sensor的设置,如下所示
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>
<sensor name="ft_sensor">
<state_interface name="force.x" />
<state_interface name="force.y" />
<state_interface name="force.z" />
<state_interface name="torque.x" />
<state_interface name="torque.y" />
<state_interface name="torque.z" />
<param name="frame_id">ft_sensor_link</param>
<param name="fx_range">100</param>
<param name="fy_range">100</param>
<param name="fz_range">100</param>
<param name="tx_range">15</param>
<param name="ty_range">15</param>
<param name="tz_range">15</param>
</sensor>
</ros2_control>
然后将我们的yaml文件作为参数引入进去
这里的路径一定要改成你自己的yaml文件
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find rokae_bringup)/config/rokae_ros2_controller.yaml</parameters>
</plugin>
</gazebo>
最后编写launch文件
这样你就可以成功使用ros2control获得gazebo六维力传感器信息了,本教程到此结束。
不,骗你的。
接下来才是最难的部分
重新修改URDF文件
不出意外的话当你完成了以上任务,你的gazebo可以显示模型并且ros2 control已经可以正常使用了,如下图但是当我们使用 ros2 control list_controllers的时候会发现传感器的controller压根没有启动
笔者同时也发现在gazebo启动终端中也会发生warning,但是不会报错
打开rqt也获取不到任何信息
这里笔者查阅了一些资料发现是sensor没有定义好,于是在csdn上进行搜索如何在gazebo定义六维力传感器,大部分教程会告诉你要设置plugin name=“ft_sensor” filename="libgazebo_ros_ft_sensor.so"代码块,可是这样是通过libgazebo_ros_ft_sensor.so插件的方式将六维力传感器的数据以topic形式发送出来,那么如何使用ros2 control的方式看到呢。我们可以在终端中看到端倪,ros2control 并没有找到我们的六维力传感器ft_sensor,所以是我们的六维力传感器没有定义好,翻看gazebo官网的教程Force/Torque Sensor
我们从这里可以看出来force torque sensor是在关节里面的,必须要对关节定义好,定义的地方是关节(joint)而不是连杆(link)。由于最后和传感器相连的关节类型是fixed,gazebo那边不可以使用这种类型的关节作为六维力传感器,所以添加以下代码
<gazebo
reference="joint_sensor">
<provideFeedback>true</provideFeedback>
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
其中joint_sensor是你定义的连接六维力传感器的关节
然后对其添加六维力传感器
<gazebo reference="joint_sensor">
<gravity>true</gravity>
<sensor name="ft_sensor" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
</sensor>
</gazebo>
这样就可以使用ros2 control在gazebo里添加六维力传感器了。
我们需要注意以下几点:
大部分教程告诉你要添加
plugin name=“ft_sensor” filename="libgazebo_ros_ft_sensor.so"代码块
不要添加,首先是其参数在ros2中有所变化,其次是他会和force_torque_broadcaster这个controller产生冲突,无法正确捕获到传感器。
最终可以显示的形似如下所示,其在gazebo里面可以正确显示而且在rqt中也可以看到ros2 control的数据
完结撒花
还有一点,csdn上的vip文章能不能少一点,找资料真的很难找啊!