Simulating UR5 + PR2 gripper in Gazebo

edit

I was able to plan and execute trajectories for UR5 in Gazebo. I attached PR2 gripper and was able to send messages to topic "gripper_controller/command". But now I want to have an action interface for the gripper. I am using GripperCommandAction for this purpose.

Firstly, is it possible to use GripperCommand action for Pr2 grippers? I am unable to connect to the GripperCommand action.

Following are my config and launch files :

controllers.yaml (Moveit!)

controller_list:
    - name: ur5_six_arm/arm_controller
      action_ns: follow_joint_trajectory
      default: true
      type: FollowJointTrajectory
      joints: [ur5_arm_shoulder_pan_joint, ur5_arm_shoulder_lift_joint, ur5_arm_elbow_joint, ur5_arm_wrist_1_joint, ur5_arm_wrist_2_joint, ur5_arm_wrist_3_joint, ee_joint]

    - name: ur5_six_arm/gripper_controller
      action_ns: gripper_action
      default: true
      type: GripperCommand
      joints: [r_gripper_l_finger_joint]

control_ur5.yaml - Gazebo

arm_controller:
    type: effort_controllers/JointTrajectoryController
    topic: "test"
    joints:
       - ur5_arm_shoulder_pan_joint
       - ur5_arm_shoulder_lift_joint
       - ur5_arm_elbow_joint
       - ur5_arm_wrist_1_joint
       - ur5_arm_wrist_2_joint
       - ur5_arm_wrist_3_joint
    gains:
        ur5_arm_shoulder_pan_joint: {p: 4000.0, i: 1000.0, d: 100.0, i_clamp_max: 10.0, i_clamp_min: -10.0}
        ur5_arm_shoulder_lift_joint: {p: 2000.0, i: 500.0, d: 100.0, i_clamp_max: 10.0, i_clamp_min: -10.0}
        ur5_arm_elbow_joint: {p: 1500.0, i: 500.0, d: 50.0, i_clamp_max: 10.0, i_clamp_min: -10.0}
        ur5_arm_wrist_1_joint: {p: 700.0, i: 200.0, d: 10.0, i_clamp: 0.0}
        ur5_arm_wrist_2_joint: {p: 700.0, i: 200.0, d: 10.0, i_clamp: 0.0}
        ur5_arm_wrist_3_joint: {p: 700.0, i: 200.0, d: 10.0, i_clamp: 0.0}
    constraints:
        goal_time: 0.6
        stopped_velocity_tolerance: 0.5
        ur5_arm_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
        ur5_arm_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
        ur5_arm_elbow_joint: {trajectory: 0.1, goal: 0.1}
        ur5_arm_wrist_1_joint: {trajectory: 0.1, goal: 0.1}
        ur5_arm_wrist_2_joint: {trajectory: 0.1, goal: 0.1}
        ur5_arm_wrist_3_joint: {trajectory: 0.1, goal: 0.1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10

gripper_controller:
    # Position Controllers ---------------------------------------
    type: effort_controllers/GripperActionController
    joint: r_gripper_l_finger_joint
    gains:
        r_gripper_l_finger_joint: {p: 100.0, i: 0.01, d: 10.0}

And then launching the controllers in ur5_six_arm namespace :

<!-- Load UR5 controllers -->
<rosparam ns="ur5_six_arm" command="load" file="$(find jsk_mbzirc_tasks)/config/control_ur5.yaml" />
<node ns="ur5_six_arm" name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller gripper_controller"/>

The arm_controller gets added in the controller list with the following message :

Added FollowJointTrajectory controller for ur5_six_arm/arm_controller

Controller manager lists the gripper controller :

 $rosservice call /ur5_six_arm/controller_manager/list_controllers
  controller: 
  - 
    name: arm_controller
    state: running
    type: effort_controllers/JointTrajectoryController
    hardware_interface: hardware_interface::EffortJointInterface
    resources: ['ur5_arm_elbow_joint', 'ur5_arm_shoulder_lift_joint', 'ur5_arm_shoulder_pan_joint', 'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint']
  - 
    name: gripper_controller
    state: running
    type: effort_controllers/GripperActionController
    hardware_interface: hardware_interface::EffortJointInterface
    resources: ['r_gripper_l_finger_joint']

I can see the action server started in rostopics:

/ur5_six_arm/gripper_controller/gripper_action/cancel
/ur5_six_arm/gripper_controller/gripper_action/feedback
/ur5_six_arm/gripper_controller/gripper_action/goal
/ur5_six_arm/gripper_controller/gripper_action/result
/ur5_six_arm/gripper_controller/gripper_action/status

I have written a basic action client just to see if I can connect to the server:

client = actionlib.SimpleActionClient('ur5_six_arm/gripper_controller/gripper_action', GripperCommandAction)
#client.wait_for_server():
rospy.loginfo("Waiting for gripper action server")
if client.wait_for_server(rospy.Duration(1)):
    rospy.logwarn("Found gripper action server")
else:
    rospy.logwarn("Cannot find gripper action server")

But action client fails to connect to gripper action server :

MoveitSimpleControllerManager: Waiting for ur5_six_arm/gripper_controller/gripper_action to come up MoveitSimpleControllerManager: Action client not connected: ur5_six_arm/gripper_controller/gripper_action

Controller Spawner couldn't find the expected controller_manager ROS interface.

Sorry about the long post ... (more)