Simulating UR5 + PR2 gripper in Gazebo

asked 2016-04-28 03:40:30 -0600

ipa-hsd gravatar image

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)

edit retag flag offensive close merge delete

Comments

OK finally got this running! The action name is not gripper_action but gripper_cmd client = actionlib.SimpleActionClient('ur5_six_arm/gripper_controller/gripper_cmd', GripperCommandAction). There's no documentation for this. Can I make one?

ipa-hsd gravatar imageipa-hsd ( 2016-04-28 04:48:34 -0600 )edit

How can you managed to attach pr2 gripper? I'am also trying to attach, but gripper is not working with moveit. Can you share your urdf file?

mhmmtucan gravatar imagemhmmtucan ( 2017-12-04 03:27:55 -0600 )edit

I am trying to add a custom 1 dof to the already existing robot;i did same as what you did but in controller_manager list there is no hardware interface and resources for gripper.i checked the transmission,its right. should i change something more in the gripper_action_controller pkg,can i see yours

naveen010 gravatar imagenaveen010 ( 2018-04-17 04:48:47 -0600 )edit

Hi, Can u please explain how to add pr2 gripper with UR5 robot. I am able to control the UR5 ,both in simulation and actual robot using moveIt.Yet to figure out a way to add gripper n the same

rajmohan747 gravatar imagerajmohan747 ( 2018-05-13 09:10:30 -0600 )edit