joint_position_controller for robotiq gripper (on a UR5) in Moveit

[Philip W]

Hi guys and gals,

I'm working on integrating the UR5 arm together with a robotiq 2-finger gripper in Moveit using ros_control. The UR5 is working with the modern_ur_driver, but I'm having trouble with the gripper. I'm using the robotiq_2f controller from here: https://github.com/beta-robots/robotiq

When the gripper is open, I can close it once then nothing happens anymore:

[ INFO] [1513867635.788159560]: ParallelPlan::solve(): Solution found by one or more threads in 0.002917 seconds
[ INFO] [1513867635.790615347]: SimpleSetup: Path simplification took 0.002361 seconds and changed from 6 to 2 states
[ WARN] [1513867642.646593250]: Controller handle /simple_gripper/joint_position_controller reports status ABORTED
[ INFO] [1513867642.821323582]: ABORTED: Solution found but controller failed during execution
[ INFO] [1513867650.682330631]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1513867650.682376119]: Goal constraints are already satisfied. No need to plan or execute any motions

(Here is the complete log: https://www.dropbox.com/s/cmim7vvwck1vz9w/current_console_output.txt?dl=0)

Now I've heard that Moveit doesn't work with the joint_position_controller. Is that correct and what can I do to get it to run?

Here is what I have right now:

The launch file:

<launch>
  <arg name="robot_ip" default="192.168.20.77"/>
  <arg name="limited" default="true"/>
  <arg name="min_payload"  default="0.0"/>
  <arg name="max_payload"  default="3.0"/>
  <arg name="prefix" default="simple_arm_" />  
  <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->


 


  <!-- robot model -->
  <include file="$(find myrobot2_description)/launch/myrobot2_upload.launch">
    <arg name="limited" value="$(arg limited)"/>
  </include>


  <!--UR5 modern driver-->
  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="log" launch-prefix="">
    <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
    <param name="min_payload" type="double" value="$(arg min_payload)"/>
    <param name="max_payload" type="double" value="$(arg max_payload)"/>
    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
    <param name="use_ros_control" type="bool" value="True"/>
    <param name="servoj_gain" type="double" value="750" />
    <param name="prefix" value="$(arg prefix)" />
  </node>


  <!-- Load controller settings -->
  <rosparam file="$(find myrobot2_driver)/config/myrobot2_controllers.yaml" command="load"/>




  <!-- robotiq gripper -->
  <group ns="simple_gripper">
 
    <rosparam file="$(find myrobot2_driver)/config/gripper_controller.yaml" command="load" />
 
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_position_controller"/>
 
    <node name="simple_gripper_hw" pkg="robotiq_2f_hw_usb" type="robotiq_2f_hw_usb_node" respawn="false" output="screen">
    <param name="port" type="str" value="/dev/ttyUSB0"/>
    <param name="server_id" type="int" value="9"/>
    <param name="name" type="str" value="simple_gripper_"/>
    </node>
 
  </group>




  <!-- spawn controller manager -->
  <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />


  <!-- load other controller -->
  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="load pos_based_pos_traj_controller" />


  <!-- Convert joint states to /tf tranforms -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>


</launch>

This is the gripper_controller.yaml I'm loading:

hardware_control_loop:
   loop_hz: 125


# Settings for ros_control hardware interface
hardware_interface:
   joints:
     - simple_gripper_robotiq_85_left_knuckle_joint


#simple_gripper:
joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 125  #100


joint_position_controller:
   type: position_controllers/GripperActionController
   joint: simple_gripper_robotiq_85_left_knuckle_joint


joint_trajectory_controller:
   type: position_controllers/JointTrajectoryController
   joints:
     - simple_gripper_robotiq_85_left_knuckle_joint
   constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      simple_gripper_robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1}
   stop_trajectory_duration: 0.5
   state_publish_rate:  25
   action_monitor_rate: 10

And this is the controllers.yaml for Moveit:

#----------------------Gripper--------------------------
  - name: joint_position_controller
    action_ns: gripper_cmd
    type: GripperCommand
    default: true
    joints:
      - simple_gripper_robotiq_85_left_knuckle_joint


#-----------------------UR5-------------------------


  - name: vel_based_pos_traj_controller #or pos_based_pos_traj_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - simple_arm_shoulder_pan_joint
      - simple_arm_shoulder_lift_joint
      - simple_arm_elbow_joint
      - simple_arm_wrist_1_joint
      - simple_arm_wrist_2_joint
      - simple_arm_wrist_3_joint

I'll gladly provide any additional information you might need and appreciate any help!

Thanks!