Panda gripper reports status aborted when grasping object

[YannickRiou]

When using real panda robot with moveit task constructor, I run into a problem with the gripper with it tries to grasp an object.
Here is the error :

[ INFO] [1641286631.959876387]: Planner configuration 'panda_arm[RRTConnect]' will use planner 'geometric::RRTConnect'.   Additional configuration parameters will be set when the planner is constructed.  
[ INFO] [1641286631.960346854]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.960388674]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.960445938]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure 
[ INFO] [1641286631.960636115]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.980818876]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286631.982119349]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286631.990055423]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286631.991240341]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286631.991327507]: ParallelPlan::solve(): Solution found by one or more threads in 0.031149 seconds  
[ INFO] [1641286631.991595939]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.991655853]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.992194537]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286631.994029386]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286632.002312625]: panda_arm[RRTConnect]: Created 5 states (3 start + 2 goal)  
[ INFO] [1641286632.002680854]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286632.002772103]: panda_arm[RRTConnect]: Created 5 states (2 start + 3 goal)  
[ INFO] [1641286632.007846156]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286632.007996674]: ParallelPlan::solve(): Solution found by one or more threads in 0.016450 seconds  
[ INFO] [1641286632.008241504]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286632.008333875]: panda_arm[RRTConnect]: Starting planning with 1 states already in datastructure  
[ INFO] [1641286632.014690730]: panda_arm[RRTConnect]: Created 4 states (2 start + 2 goal)  
[ INFO] [1641286632.015197475]: panda_arm[RRTConnect]: Created 5 states (3 start + 2 goal)  
[ INFO] [1641286632.015299907]: ParallelPlan::solve(): Solution found by one or more threads in 0.007125 seconds  
[ INFO] [1641286632.043890726]: SimpleSetup: Path simplification took 0.028506 seconds and changed from 3 to 2 states  
[ INFO] [1641286635.360428419]: Controller position_joint_trajectory_controller successfully finished  
[ INFO] [1641286635.458569741]: Completed trajectory execution with status SUCCEEDED ...  
[ INFO] [1641286645.501356564]: Executing TaskSolution  
[ INFO] [1641286649.302192578]: Controller position_joint_trajectory_controller successfully finished  
[ INFO] [1641286651.332638692]: Controller position_joint_trajectory_controller successfully finished  
[ WARN] [1641286652.336841865]: Controller handle franka_gripper reports status ABORTED  
[ INFO] [1641286652.336899522]: Completed trajectory execution with status ABORTED ...

Here is the code that create the task : https://gist.github.com/YannickRiou/f84a52efa3acbdb061ce82b38a76c97e
The gripper_planner_ is a joint interpolation planner (it worked well on the PR2 robot) :
gripper_planner_ = std::make_sharedsolvers::JointInterpolationPlanner();

The problems seems to be that the gripper cannot close fully (as asked by the command in the task creation) because of the object, so the controller isn't happy as the command cannot reach the fully closed goal.

It seems to be a common known problem but I haven't found any solution.

Thanks in advance.

[rhaschke]

@rickstaa, did you find a good solution to operate the Panda gripper?

[rickstaa]

@rhaschke I did not perform any tests on the real robot to set the gripper width programmatically. I only used the open and closed group states defined in the panda MoveIt configuration or performed direct gripper control.

https://github.com/ros-planning/panda_moveit_config/blob/b645ff68d75e571be859721aa376930dac9821c7/config/hand.xacro#L30-L37

I know @marcbone is currently working on the simulated gripper performance (see this comment). In the simulated case, there are two problems. First, the PID gains for the griper need to be improved for the gripper grasp action to work (see frankaemika/franka_ros#173 (comment)). Further, this pull request needs to be merged before MoveIt can control the panda gripper in the simulation.

I am, however, not familiar with gripper problems on the real robot. @YannickRiou a quick workaround would be not to use MoveIt for the gripper control but use the Franka gripper actions directly. This is what I am currently doing in my scripts. If this is not feasible in your case, we could open an issue on the franka_ros repository.

[YannickRiou]

As shown in the gist in my original post, I am indeed using the open/closed pre-defined poses for the panda hand planning group in the task creation. The command is correctly sent to the real panda hand (before the pick, the gripper is correctly opened, and when grasping the gripper close correctly before going into error because it can't reach the fully close position because of the grasped object) .
So I think the problem might be on the side of the franka_ros gripper controller that returns error when grasping object.

a quick workaround would be not to use MoveIt for the gripper control but use the Franka gripper actions directly

As I am using Moveit task constructor to create my pick/place task I think this is not easily feasible as the control of the gripper is directly integrated in the whole task solution.

[rickstaa]

@YannickRiou, I see. Unfortunately, I do not have access to the real robot at the moment, so I can not help you further debug this problem. I will work with the real robot in Feb again. I think the fastest way to solve this is to open an issue on the franka_ros repository.

[rhaschke]

@YannickRiou, do you use the GripperCommand action to execute your hand trajectories?
As Rick said, Franka is working on the GripperCommand: frankaemika/franka_ros#193.

As a workaround, you could try to set a large enough goal tolerance for your trajectory controller.

[YannickRiou]

Yes I do use the GripperCommand in my panda_moveit_config/config/panda_gripper_controllers.yaml :

 - name: franka_gripper
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - panda_finger_joint1
      - panda_finger_joint2

I didn't know that this was still in developpment, I will track the issue that you mentionned and for now I will try to set a large goal tolerance as a workaround.

[YannickRiou]

Increasing theses values solved the problem :

default_grasp_epsilon:
  inner: 0.08 # [m]
  outer: 0.08 # [m]

in the following configuration file franka_ros/franka_gripper/config/franka_gripper_node.yaml

[rickstaa]

Great, for future reference, a detailed discussion about why this works can be found here.