moveit!控制双UR3e机械臂

需要把实验室的两台UR3e拼成一个双臂机器人一起做控制。之前从一个双臂开源项目中得到了双UR10的moveit!功能包，经过修改，能够在Gazebo中对双UR3e机器人进行路径规划和控制仿真。但在应用到实际机器人中的时候遇到了各种困难。
机械臂驱动：UR3e需要使用最新的ur_robot_driver驱动，而开源项目中使用了ur_modern_driver驱动，这导致需要自己搭建双机械臂驱动。这需要对ros_control有更好的理解。 根据原有的ur3e_bringup.launch进行修改，使用两个节点分别控制两个机械臂，在创建节点时，曾通过在节点名之前加前缀的方式区分节点（如：left_)，经实践后发现这样改会遇到很多麻烦。
然后，建立双机械臂的URDF模型，这一步在之前用gazebo仿真的时候已经完成，过程基本类似。
另外，需要修改ur3e_controllers.yaml。我在这一步困了两周，尝试了很多种方式，最后找到了一种解决方法。分别编写两个yaml(如：left_controller.yaml和right_controller.yaml)，以其中一个为例：

left_arm:			# 在整个文件前写上机器人名称，可以给下面所有定义的控制器加上类名
   # Settings for ros_control control loop
   hardware_control_loop:
      loop_hz: &loop_hz 500

   # Settings for ros_control hardware interface
   #left_ur_hardware_interface:
   ur_hardware_interface:		# 程序会在URDF中寻找同名的关节，因此需要在每个关节前加上前缀
      joints: &robot_joints
      - left_shoulder_pan_joint
      - left_shoulder_lift_joint
      - left_elbow_joint
      - left_wrist_1_joint
      - left_wrist_2_joint
      - left_wrist_3_joint

   # Publish all joint states ----------------------------------
   joint_state_controller:
      type:         joint_state_controller/JointStateController
      publish_rate: *loop_hz

   # Publish wrench ----------------------------------
   force_torque_sensor_controller:
      type:         force_torque_sensor_controller/ForceTorqueSensorController
      publish_rate: *loop_hz

   # Publish speed_scaling factor
   speed_scaling_state_controller:
      type:         ur_controllers/SpeedScalingStateController
      publish_rate: *loop_hz

   # Joint Trajectory Controller - position based -------------------------------
   # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
   scaled_pos_joint_traj_controller:
      type: position_controllers/ScaledJointTrajectoryController
      joints: *robot_joints
      constraints:
         goal_time: 0.6
         stopped_velocity_tolerance: 0.05
         left_shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
         left_shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
         left_elbow_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_1_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_2_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_3_joint: {trajectory: 0.2, goal: 0.1}
      stop_trajectory_duration: 0.5
      state_publish_rate: *loop_hz
      action_monitor_rate: 20

   pos_joint_traj_controller:
      type: position_controllers/JointTrajectoryController
      joints: *robot_joints
      constraints:
         goal_time: 0.6
         stopped_velocity_tolerance: 0.05
         left_shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
         left_shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
         left_elbow_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_1_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_2_joint: {trajectory: 0.2, goal: 0.1}
         left_wrist_3_joint: {trajectory: 0.2, goal: 0.1}
      stop_trajectory_duration: 0.5
      state_publish_rate: *loop_hz
      action_monitor_rate: 20

   scaled_vel_joint_traj_controller:
      type: velocity_controllers/ScaledJointTrajectoryController
      joints: *robot_joints
      constraints:
         goal_time: 0.6
         stopped_velocity_tolerance: 0.05
         left_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
         left_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
         left_elbow_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_1_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_2_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_3_joint: {trajectory: 0.1, goal: 0.1}
      gains:
         #!!These values have not been optimized!!
         left_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
      # Use a feedforward term to reduce the size of PID gains
      velocity_ff:
         left_shoulder_pan_joint: 1.0
         left_shoulder_lift_joint: 1.0
         left_elbow_joint: 1.0
         left_wrist_1_joint: 1.0
         left_wrist_2_joint: 1.0
         left_wrist_3_joint: 1.0
      stop_trajectory_duration: 0.5
      state_publish_rate: *loop_hz
      action_monitor_rate: 20

   vel_joint_traj_controller:
      type: velocity_controllers/JointTrajectoryController
      joints: *robot_joints
      constraints:
         goal_time: 0.6
         stopped_velocity_tolerance: 0.05
         left_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
         left_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
         left_elbow_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_1_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_2_joint: {trajectory: 0.1, goal: 0.1}
         left_wrist_3_joint: {trajectory: 0.1, goal: 0.1}
      gains:
         #!!These values have not been optimized!!
         left_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
         left_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
      # Use a feedforward term to reduce the size of PID gains
      velocity_ff:
         left_shoulder_pan_joint: 1.0
         left_shoulder_lift_joint: 1.0
         left_elbow_joint: 1.0
         left_wrist_1_joint: 1.0
         left_wrist_2_joint: 1.0
         left_wrist_3_joint: 1.0
      stop_trajectory_duration: 0.5
      state_publish_rate: *loop_hz
      action_monitor_rate: 20

   # Pass an array of joint velocities directly to the joints
   joint_group_vel_controller:
      type: velocity_controllers/JointGroupVelocityController
      joints: *robot_joints

   robot_status_controller:
      type: industrial_robot_status_controller/IndustrialRobotStatusController
      handle_name: industrial_robot_status_handle
      publish_rate: 10

这样在运行bringup之后可以得到类名相同的两组param和node

NODES
  /left_arm/
    controller_stopper (controller_stopper/node)
    ros_control_controller_spawner (controller_manager/spawner)
    ros_control_stopped_spawner (controller_manager/spawner)
    ur_hardware_interface (ur_robot_driver/ur_robot_driver_node)
    ur_robot_state_helper (ur_robot_driver/robot_state_helper)
  /right_arm/
    controller_stopper (controller_stopper/node)
    ros_control_controller_spawner (controller_manager/spawner)
    ros_control_stopped_spawner (controller_manager/spawner)
    ur_hardware_interface (ur_robot_driver/ur_robot_driver_node)
    ur_robot_state_helper (ur_robot_driver/robot_state_helper)
  /
    joint_state_publisher (joint_state_publisher/joint_state_publisher)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)

还遇到了controller的名字不一样的问题。需要保证server端和client端的名称保持一致（具体一下哪边是client哪边是server）
参考：https://blog.csdn.net/ha010/article/details/109274459
https://blog.csdn.net/qq_50598558/article/details/114702163

另外出现了WARN:Controller Spawner couldn’t find the expected controller_manager ROS interface.的问题，这是因为前面提到的命名空间不统一的问题。
参考：https://blog.csdn.net/CCsLife/article/details/106086854

目前的情况是，打开bringup和moveit没有问题，执行路径规划也没有问题，但是无法执行，控制真是机械臂。
报错如下：Controller right_arm/scaled_pos_joint_traj_controller failed with error code PATH_TOLERANCE_VIOLATED
这个问题还未解决。

3月31日，问题解决了！
PATH_TOLERANCE_VIOLATED问题出现之后的某天突然想到，rviz中的机器人模型并没有和实际机器人同步，还是最开始的那个姿势，所以一定是joint_states出现了问题。
查看joint_states的话题内容，发现都是0，难怪会出现超过约束的问题。
查看joint_states的info，发现根本没有发布者，只有订阅者。
查看目前的双臂驱动rqt_graph:

和能正常使用的单机械臂驱动的rqt_graph比较之后发现了两个问题：
(1)驱动节点有一些不一样的地方，应该是这样：

这个问题出在了bringup的最后：
注释中提示
Make sure to start this in the namespace of the hardware interface
而我并没有把该节点在ur_hardware_interface命名空间中，应修改为：

  <!-- Make sure to start this in the namespace of the hardware interface -->
  <node ns="right_arm/ur_hardware_interface" name="ur_robot_state_helper" pkg="ur_robot_driver" type="robot_state_helper" respawn="false" output="screen">
  </node>

  <!-- Make sure to start this in the namespace of the hardware interface -->
  <node ns="left_arm/ur_hardware_interface" name="ur_robot_state_helper" pkg="ur_robot_driver" type="robot_state_helper" respawn="false" output="screen">
  </node>

修改之后，仍然没有joint_states
查看rostopic list，发现原来是节点发布的话题多了一个类名！

马上加上remap

<remap from="/left_arm/joint_states" to="/joint_states"/>

大功告成！

折磨了我近一个月的双臂驱动终于完成了。