moveit运动规划和gazebo仿真的踩坑记录

本文主要记录踩过的坑

环境：ubuntu18、ROS Melodic

1.MoveIt Setup Assistant

rosrun moveit_setup_assistant moveit_setup_assistant

moveit配置助手启动后，右下角图片位置显示为2.0版本。与网上的教程版本有些不同的地方。

这里我选择新建方式，加载了手写的xcaro文件。

1.1 Planning Group

在Planning Group 标签页，相比其他教程的图片少了Kin.Solver Attempts的选项。我这里给机械臂的规划组取名为arm_pg。OMPL Planning 选择 RRT。

1.2 ROS Control 和 Simulation

这两个标签页都选择了自动生成。

ROS Control 根据Planning Groups 中的arm_pg规划组，自动生成了arm_pg_controller。

配置助手导入的xacro文件中，并未设置Transmissions和gazebo的gazebo_ros_controller元素。源xacro文件中只设置了颜色元素。如下：

<gazebo reference="base_link">
    <material>Gazebo/White</material>
</gazebo>

Simulation标签页中，点击自动生成，可生成带有transmission标签和gazebo_ros_control的文件，以满足gazebo仿真需要。后续进行gazebo仿真时，可以用生成的文件替换原文件。

<transmission name="trans_lj6_joint">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lj6_joint">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lj6_joint_motor">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>
<gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
        <robotNamespace>/</robotNamespace>
    </plugin>
</gazebo>

自动生成的文件中，硬件接口中为EffortJointInterface，类型不匹配（命令行输出报错，且gazebo中机械臂受重力影响，耷拉着。），手动替换为PositionJointInterface。如下：

<transmission name="trans_lj6_joint">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lj6_joint">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="lj6_joint_motor">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

1.3 在rviz中进行moveit运动规划

在Configuration Files标签页，选择合适的文件夹，生成配置文件。这里以文件夹my_s6_config为例，将生成的配置文件保存在该文件夹中。

保存成功后退出配置助手，运行以下命令，在可视化工具rviz中用moveit进行规划。

roslaunch my_s6_config demo.launch

2. gazebo仿真准备

步骤主要参照这篇文章《协作臂学习(6)-五步搞定：基于Moveit_config配置gazebo仿真》

协作臂学习(6)-五步搞定：基于Moveit_config配置gazebo仿真_酷比程序的博客-CSDN博客

2.1修改ros_controllers.yaml

路径：my_s6_config/config/ros_controllers.yaml

添加如下内容，其中arm_pg_controller对应之前movite配置时，ROS Control中根据规划组arm_pg自动生成的名字 arm_pg_controller。

各关节对应urdf文件中的关节名称。

arm_pg_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - lj1_joint
    - lj2_joint
    - lj3_joint
    - lj4_joint
    - lj5_joint
    - lj6_joint
gazebo_ros_control/pid_gains:
#  gains:
    lj1_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
    lj2_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
    lj3_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
    lj4_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
    lj5_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}
    lj6_joint: {p: 100.0, i: 1.0, d: 10.0, i_clamp_min: -1.0, i_clamp_max: 1.0}

2.2 修改robot_name_moveit_controller_manager.launch.xml

文件路径：my_s6_config/launch/robot_name_moveit_controller_manager.launch.xml

添加一行

<launch>
    <!-- loads moveit_controller_manager on the parameter server which is taken as argument
    if no argument is passed, moveit_simple_controller_manager will be set -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

    <arg name="execution_type"/>

    <!-- loads ros_controllers to the param server -->
    <rosparam file="$(find my_s6_config)/config/ros_controllers.yaml"/>
</launch>

2.3 修改demo.launch

文件路径：my_s6_config/launch/demo.launch

把fake_execution改为false，对rviz来说gazebo也是算是真机。真机模式rviz会尝试连接机器人控制器。

<!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find my_s6_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/>
    <arg name="execution_type" value="$(arg execution_type)"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="pipeline" value="$(arg pipeline)"/>
    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>

2.4 修改ros_controllers.launch

文件路径：my_s6_config/launch/ros_controllers.launch

将ROS Control中根据规划组arm_pg自动生成的名字 arm_pg_controller，填入args中，对应ros_controllers.yaml中的arm_pg_controller。

<launch>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find my_s6_config)/config/ros_controllers.yaml" command="load"/>

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="arm_pg_controller"/>
</launch>

2.5 替换原urdf

在moveit配置助手中的Simulation标签页中，可自动生成用gazebo仿真的urdf，这里我们用来替换掉原urdf文件中的内容。

如1.2所提到的，可能需要修改中的内容。

3. 启动moveit + gazebo

执行如下命令

roslaunch my_s6_config demo_gazebo.launch 

在rviz中用moveit规划路径并执行后，就能在gazebo中看到执行结果了。
这里运动后，重心不稳，倒了。。。。

执行以下命令后，可以看到有follow_joint_trajectory。

rostopic list

如果是在虚拟机中启动gazebo出现黑屏的状况，需关闭虚拟机的3D加速。关闭3D加速后，gazebo就能正常显示场景了（好卡。），如果是gazebo缺失model文件，就需要下载相关model文件了。