Moveit+Gazebo仿真

控制架构

重点是完成控制接口两端的Follow Joint Trajectory、Joint Trajectory Controller、Joint State Controller配置。

Follow Joint Trajectory：作为action通信的client端，将Moveit!规划好的轨迹发给Gazebo。

Joint Trajectory Controller：这是action通信的server端，其中的关节名称必须要与机器人模型中的joint名称一致，并通过launch文件加载配置参数并启动控制器。

Joint State Controller：以50Hz的频率发布机器人的关节状态。

yaml文件配置

每一个冒号对应一级参数命名空间，如需添加多个机器人则要在前面再加一级（可以命名为机器人的名字），并在launch文件里面做相应改动（ns、remap）。注意理解参数命名空间的概念，具体详见yaml文件配置。三个控制器设置分别如下：

Follow Joint Trajectory：

controller_list:
  - name: arm_joint_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - joint_1_s
      - joint_2_l
      - joint_3_u
      - joint_4_r
      - joint_5_b
      - joint_6_t

Joint Trajectory Controller：

arm_joint_controller:
  type: "position_controllers/JointTrajectoryController"
  joints:
    - joint_1_s
    - joint_2_l
    - joint_3_u
    - joint_4_r
    - joint_5_b
    - joint_6_t

  gains:
    joint_1_s: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint_2_l: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint_3_u: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint_4_r: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint_5_b: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
    joint_6_t: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

Joint State Controller：

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

其中，Joint State Controller和Follow Joint Trajectory已经使用MoveIt Setup Assistant自动生成，具体为config文件夹下的ros_controllers。我们只需要新建一个yaml文件填写剩下的Joint Trajectory Controller即可（该例中路径为$(find motoman_gazebo)/config/mh12_joint_trajectory_controller.yaml）。

launch文件配置

启动Moveit

参见下文

Moveit主要launch及其配置

启动控制器

# 使用rosparam加载yaml文件里面定义的参数，注意修改包名及路径
<rosparam file="$(find motoman_mh12_moveit_config)/config/ros_controllers.yaml" command="load"/>
<rosparam file="$(find motoman_gazebo)/config/mh12_joint_trajectory_controller.yaml" command="load"/>

# 启动控制器，注意后两个的参数项为对应的控制器名称，与yaml文件对应
<node name="robot_state_publisher" type="robot_state_publisher" pkg="robot_state_publisher"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_joint_controller"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller"/>

启动Gazebo

<include file = "$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="headless" value="$(arg headless)"/>
</include>

<node name="mh12_gazebo_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" 
    args="-file $(arg urdf_path) -urdf -model mh12"/>

直接照抄gazebo.launch里面对应内容即可。

可能碰到的问题

Could not find joint in 'hardware_interface::PositionJointInterface'.

解决方法：修改urdf文件里面的<hardwareInterface>项为<hardwareInterface>hardware_interface/PositionJointInterface<hardwareInterface>

No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/

解决方法：无视即可，不影响使用。产生的原因为初始化RobotHWSim的时候robot_ros_control会加载对应参数。如果有的话就使用pid控制器，否则就使用gazebo的其他控制方法。