How to integrate PR2 gripper with MoveIt & Gazebo

edit

Hello,

I'm working on a project with MoveIt in ROS Kinetic and Ubuntu 16.04. The objective is to move a PR2 arm in Gazebo7 using MoveIt in order to complete a pick & place application. Pick & place is working fine launching Rviz without Gazebo. Trajectory is planned, right arm picks the object and move it to another position in the table. After that I've tried to configure all necessary controllers and launch files to make it work in the simulator(Gazebo) but even if I can plan trajectories with the right arm, right gripper isn't working.

First of all I launch Gazebo with the following command: roslaunch pr2_gazebo pr2_empty_world.launch

The controllers.yaml file:

controller_manager_ns: pr2_controller_manager
controller_list:
 - name: r_arm_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
     - r_shoulder_pan_joint
     - r_shoulder_lift_joint
     - r_upper_arm_roll_joint
     - r_elbow_flex_joint
     - r_forearm_roll_joint
     - r_wrist_flex_joint
     - r_wrist_roll_joint

 - name: r_gripper_controller
   action_ns: gripper_action    
   type: GripperCommand     
   default: true
   joints:
     - r_gripper_motor_screw_joint
     - r_gripper_motor_slider_joint
     - r_gripper_l_finger_joint   
     - r_gripper_r_finger_joint   
     - r_gripper_r_finger_tip_joint 
     - r_gripper_l_finger_tip_joint 
     - r_gripper_palm_joint 
     - r_gripper_led_joint
     - r_gripper_motor_accelerometer_joint
     - r_gripper_tool_joint
     - r_gripper_joint

This joints are also defined in joint_names.yaml.

Logs from RVIZ:

[ INFO] [1528755561.122395255, 14.094000000]: Execution request received for ExecuteTrajectory action.
[ERROR] [1528755599.629955608, 19.566000000]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 5.464108 seconds). Stopping trajectory.
[ INFO] [1528755599.630070156, 19.566000000]: Cancelling execution for r_gripper_controller
[ INFO] [1528755599.645815043, 19.571000000]: Execution completed: TIMED_OUT
[ INFO] [1528755599.649971933, 19.571000000]: ABORTED: Timeout reached

Then I tried adding this lines to the controllers in controllers.yaml:

   constraints:
     - goal_time: 0.5

This just hide the timeout error, but execution lasts forever and nothing happens.

Some additional info:

$ rosrun pr2_controller_manager pr2_controller_manager list
base_controller ( running )
base_odometry ( running )
head_traj_controller ( running )
l_arm_controller ( running )
l_gripper_controller ( running )
laser_tilt_controller ( running )
r_arm_controller ( running )
r_gripper_controller ( running )
torso_controller ( running )

$ rostopic list 
/r_gripper_controller/command
/r_gripper_controller/gripper_action/cancel
/r_gripper_controller/gripper_action/feedback
/r_gripper_controller/gripper_action/goal
/r_gripper_controller/gripper_action/result
/r_gripper_controller/gripper_action/status
/r_gripper_controller/pid/parameter_descriptions
/r_gripper_controller/pid/parameter_updates

RVIZ logs at start:

[ INFO] [1528755770.703896635, 45.914000000]: Added FollowJointTrajectory controller for r_arm_controller
[ INFO] [1528755771.015454273, 45.968000000]: Added GripperCommand controller for r_gripper_controller

Gazebo logs at Start:

[INFO] [1528755476.047959, 0.954000]: Started controllers: base_controller, base_odometry, head_traj_controller, laser_tilt_controller, torso_controller, r_gripper_controller, l_gripper_controller, r_arm_controller, l_arm_controller

Summary: I launch PR2 arm in Gazebo. I launch RVIZ with PR2. I plan trajectories for PR2 from RVIZ. Executing trajectories for right_arm group is working as expected. But when I try to move the gripper (right_gripper group), it doesn't.

Thanks in advance. Regards.