建模实践5 —— moveit + gazebo 联合仿真_moveit在gazebo仿真

1. MoveIt！机器人控制框架

官话：moveit输入用户指令，比如起始位置，输出轨迹数据，每个点通过位置速度加速度描述，为了通过话题发出来，还需要一个Follow Joint Trajectory功能，通过通信接口发给机器人，机器人还需要对轨迹进行精插补，再驱动电机同步运动。机器人控制器还需要将每个电机的状态数据反馈给moveit来确定机器人是否到达指定位置，这样完成一个闭环。

普通话：首先当指令进来后，由 Moveit 做规划，不管是避障也好，还是关节规划也好，出来以后都是 Trajectory Data ，也就是轨迹数据，通过 Follow Joint Trajectory 这个接口，将轨迹信息封装成 Action，Action 发送给Trajectory Controller ，不论是在实际的机器人控制器还是Gazebo中，都会在这里进行插补运算（因为轨迹里边是一些中间间隔一定时间长度的轨迹点，这个时间不可以直接用做控制，控制器要求这个时间必须细微，所以就涉及到了插补算法），插补算法会完成六个轴的插补运算，插补后将数据发送给 Position Servo（位置伺服驱动器），驱动器收到信号之后，将电平信号变成电流信号、电压信号给到电机，电机发生运动。机器人端还要将状态信息通过Joint State Controller反馈给Moveit。

主要是理解三个模块的作用：Follow Joint Trajectory，Joint Trajectory Controller, Joint State Controller

gazebo这边是一个action server，moveit那边是一个action client，轨迹通过Action机制发出来，sever这一端接收轨迹，完成插补运算，再发到每一个电机位置控制接口上，所以配置Moveit + Gazebo 很主要的一点就是做到“插座”和“插头”的匹配。

2. 代码层面的理解

在 /husky_kinova_moveit_config/config/ros_controllers.yaml 中：

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: arm
  joint_model_group_pose: todo_no_pose_selected
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
  joints:
    - front_left_wheel
    - front_right_wheel
    - rear_left_wheel
    - rear_right_wheel
    - j2n6s300_joint_1
    - j2n6s300_joint_2
    - j2n6s300_joint_3
    - j2n6s300_joint_4
    - j2n6s300_joint_5
    - j2n6s300_joint_6
    - j2n6s300_joint_finger_1
    - j2n6s300_joint_finger_tip_1
    - j2n6s300_joint_finger_2
    - j2n6s300_joint_finger_tip_2
    - j2n6s300_joint_finger_3
    - j2n6s300_joint_finger_tip_3
  sim_control_mode: 1  # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - j2n6s300_joint_1
      - j2n6s300_joint_2
      - j2n6s300_joint_3
      - j2n6s300_joint_4
      - j2n6s300_joint_5
      - j2n6s300_joint_6
  - name: hand_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - j2n6s300_joint_finger_1
      - j2n6s300_joint_finger_tip_1
      - j2n6s300_joint_finger_2
      - j2n6s300_joint_finger_tip_2
      - j2n6s300_joint_finger_3
      - j2n6s300_joint_finger_tip_3
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - j2n6s300_joint_1
    - j2n6s300_joint_2
    - j2n6s300_joint_3
    - j2n6s300_joint_4
    - j2n6s300_joint_5
    - j2n6s300_joint_6
  gains:
    j2n6s300_joint_1:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_2:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_3:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_4:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_5:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_6:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
hand_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - j2n6s300_joint_finger_1
    - j2n6s300_joint_finger_tip_1
    - j2n6s300_joint_finger_2
    - j2n6s300_joint_finger_tip_2
    - j2n6s300_joint_finger_3
    - j2n6s300_joint_finger_tip_3
  gains:
    j2n6s300_joint_finger_1:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_finger_tip_1:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_finger_2:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_finger_tip_2:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_finger_3:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
    j2n6s300_joint_finger_tip_3:
      p: 100
      d: 1
      i: 1
      i_clamp: 1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136

在这里，arm_controller 和 hand_controller 将Moveit 生成的轨迹封装成 follow_joint_trajectory 类型的数据（这是一个数据结构，里边包含各个关节的轨迹点），在gazebo端，控制器arm_controller和hand_controller一定要做到和上边严格一致（也就是插头和插座的匹配），不然无法实现联合仿真。以上这个文件呢就完成了三个控制器的配置，那么还需要修改/husky_kinova_moveit_config/launch/ros_controllers.launch 文件以启动这三个控制器，将joint_state_controller arm_controller hand_controller加进来就可以了。

<?xml version="1.0"?>
<launch>

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

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

</launch>
1
2
3
4
5
6
7
8
9
10
11

3. 实际操作

使用 MoveIt Setup Assistant 生成 moveit_config 的过程参见： 使用MoveIt Setup Assistant创建husky_kinova_moveit_config功能包

生成 moveit_config 包后运行：

roslaunch husky_kinova_moveit_config demo_gazebo.launch
1

出现以下问题：

3.1 问题1

[ERROR] [1603521785.352021502, 0.234000000]: Could not find joint ‘j2n6s300_joint_1’ in ‘hardware_interface::PositionJointInterface’.
[ERROR] [1603521785.352217869, 0.234000000]: Failed to initialize the controller
[ERROR] [1603521785.352268883, 0.234000000]: Initializing controller ‘arm_controller_gazebo’ failed
[ERROR] [1603521786.353700, 0.940000]: Failed to load arm_controller_gazebo

[ERROR] [1603521786.373048378, 0.948000000]: Could not find joint ‘j2n6s300_joint_finger_1’ in ‘hardware_interface::PositionJointInterface’.
[ERROR] [1603521786.374382804, 0.949000000]: Failed to initialize the controller
[ERROR] [1603521786.374555951, 0.949000000]: Initializing controller ‘hand_contrroller_gazebo’ failed
[ERROR] [1603521787.375893, 1.521000]: Failed to load hand_contrroller_gazebo

解决办法：将模型文件中的硬件接口类型全部改为 PositionJointInterface

    <transmission name="j2n6s300_joint_1_trans">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="j2n6s300_joint_1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="j2n6s300_joint_1_actuator">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>160</mechanicalReduction>
        </actuator>
    </transmission>
1
2
3
4
5
6
7
8
9
10

3.2 问题2

[ERROR] [1603521846.052994053, 17.001000000]: Action client not connected: arm_controller/follow_joint_trajectory

解决办法：
使MoveIt控制器的命名空间与机械臂控制器插件的命名空间相同，在 /husky_kinova_moveit_config/config/ros_controllers.yaml 文件中，控制器arm_controller与命名空间arm_controller对应，控制器hand_controller与命名空间hand_controller对应。

另外，在 /husky_kinova_moveit_config/launch/ros_controllers.launch 中启动joint_state_controller、arm_controller、hand_controller

3.3 问题3

这个问题暂时忽略也可

3.4 最终联合仿真运行