Gripper open/close not synchronized during Pick

[Christian Dornhege]

Hi,

I'm trying to get Pick/Place to work on the PR2. I'm using moveit built from sources in Hydro on Precise.

I have created a simple gazebo scene with a single coke can, and added this to the planning scene using ORK object detection.

This looks fine:

I'm using the pick/place tutorial from here:

https://github.com/ros-planning/moveit_pr2/blob/hydro-devel/pr2_moveit_tutorials/pick_place/src/pick_place_tutorial.cpp

but without creating the planning scene manually, i.e. lines 156-215 are commented out and I'm passing in the correct object name from the planning scene.

The first thing I stumbled upon was that I had to change the pre_grasp_posture and grasp_posture joint name to 

g.pre_grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");

from just "r_gripper_joint" as it was before to make the gripper move at all. Is this a bug in the tutorial code or am I doing something wrong anywhere else?

Now, I'm observing the following behavior:

The arm moves in position correctly and the gripper starts to open, when I get the following:

In my client:

[ WARN] [1421259026.337060865, 5691.010000000]: Fail: ABORTED: Solution found but controller failed during execution

and in the move_group node:

[ WARN] [1421259182.528267093, 5807.111000000]: Controller handle r_gripper_controller reports status ABORTED

However, the gripper is still in the process of opening, i.e. executing the movement and that succeeds (visually, i.e. it looks open at the end).

When I then try the same thing again (now the gripper already being open), this part succeeds and the robot starts closing the gripper around the object.

The problem here is that the next motion, i.e. lifting the object already starts immediately, while the gripper is still closing. So it seems that the execution does not wait for the closing action to finish (or that action reports this incorrectly).

I'm not sure if these three problems originate from the same source or are just three independent issues. I'd be thankful for any hints as to what component might be at fault and/or how to debug and understand what is going on internally.

Best,

  Christian

[Adolfo Rodríguez Tsouroukdissian]

On Wed, Jan 14, 2015 at 8:13 PM, Christian Dornhege <c.dor...@gmail.com> wrote:

Hi,

Hey Christian,

 

I'm trying to get Pick/Place to work on the PR2. I'm using moveit built from sources in Hydro on Precise.

I have created a simple gazebo scene with a single coke can, and added this to the planning scene using ORK object detection.

This looks fine:

I'm using the pick/place tutorial from here:

https://github.com/ros-planning/moveit_pr2/blob/hydro-devel/pr2_moveit_tutorials/pick_place/src/pick_place_tutorial.cpp

but without creating the planning scene manually, i.e. lines 156-215 are commented out and I'm passing in the correct object name from the planning scene.

The first thing I stumbled upon was that I had to change the pre_grasp_posture and grasp_posture joint name to 

g.pre_grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");

from just "r_gripper_joint" as it was before to make the gripper move at all. Is this a bug in the tutorial code or am I doing something wrong anywhere else?

Now, I'm observing the following behavior:

The arm moves in position correctly and the gripper starts to open, when I get the following:

In my client:

[ WARN] [1421259026.337060865, 5691.010000000]: Fail: ABORTED: Solution found but controller failed during execution

and in the move_group node:

[ WARN] [1421259182.528267093, 5807.111000000]: Controller handle r_gripper_controller reports status ABORTED

However, the gripper is still in the process of opening, i.e. executing the movement and that succeeds (visually, i.e. it looks open at the end).

This is likely because you are asking the gripper to move faster than it can. If the controller runs out of time and has not yet reached the goal, it will abort the action goal, but still keep moving towards it. Can you specify longer durations / lower velocities for the gripper motion?.

When I then try the same thing again (now the gripper already being open), this part succeeds and the robot starts closing the gripper around the object.

The problem here is that the next motion, i.e. lifting the object already starts immediately, while the gripper is still closing. So it seems that the execution does not wait for the closing action to finish (or that action reports this incorrectly).

I haven't checked the implementation, but it might be that the lifting is either not waiting for the closing action goal to complete, or that it only waits for completion but does not condition the lifting to the success of the closing action. In the latter case, what you would see is:

- Gripper close aborts for the above reason, but gripper continues closing

- Pick logic detects close goal completed and decides to perform the next action, which is to lift

- Lifting motion takes place in parallel to the remaining part of the gripper close motion

Cheers,

Adolfo.

 

I'm not sure if these three problems originate from the same source or are just three independent issues. I'd be thankful for any hints as to what component might be at fault and/or how to debug and understand what is going on internally.

Best,

  Christian

--

Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.

[Christian Dornhege]

Hi Adolfo,

it turns out you were right on all accounts.

There are a couple things coming together, which I'll summarize in case someone stumbles upon this.

The first problem was that the gripper velocities that I get from gazebo don't match the controller config in pr2_controller_configuration_gazebo at all.

I measured values around 0.015107115682 when the gripper is moving. pr2_default_controllers.launch has the following settings:

    <node name="gripper_action_node"
          pkg="pr2_gripper_action" type="pr2_gripper_action">
      <!-- needed for stall detection in simulation with joint "jitter" -->
      <param name="stall_velocity_threshold" value="0.02" type="double"/>
      <param name="stall_timeout" value="0.5" type="double"/>
    </node>

So movement was considered stall all the time and after the stall_timeout of 0.5s every action automatically aborts. Actual jitter that I see in gazebo is around 1e-7, so setting this to 1e-6 fixed this. I'll have to test a bit

more during grasping with this, but the original 0.02 is way too high for what I see.

Now actions still abort, but after the default timeout set in moveit as you suspected. When I give an explicit timepoint in the pre_grasp_posture everything works fine.

The lifting starting immediately is caused by the implementation of pr2_moveit_controller_manager:

    // the gripper action reports failure when closing the gripper and an object is inside
    if (state == actionlib::SimpleClientGoalState::ABORTED && closing_)
      finishControllerExecution(actionlib::SimpleClientGoalState::SUCCEEDED);
    else
      finishControllerExecution(state);

With the wrong stall velocity this immediate abort caused the assumption that the object is now grasped.

Best,

  Christian

[Adolfo Rodríguez Tsouroukdissian]

On Fri, Jan 16, 2015 at 1:41 PM, Christian Dornhege <c.dor...@gmail.com> wrote:

Hi Adolfo,

it turns out you were right on all accounts.

There are a couple things coming together, which I'll summarize in case someone stumbles upon this.

Hey Martin,

Glad to hear things are clearing up.

 

The first problem was that the gripper velocities that I get from gazebo don't match the controller config in pr2_controller_configuration_gazebo at all.

I measured values around 0.015107115682 when the gripper is moving. pr2_default_controllers.launch has the following settings:

    <node name="gripper_action_node"
          pkg="pr2_gripper_action" type="pr2_gripper_action">
      <!-- needed for stall detection in simulation with joint "jitter" -->
      <param name="stall_velocity_threshold" value="0.02" type="double"/>
      <param name="stall_timeout" value="0.5" type="double"/>
    </node>

So movement was considered stall all the time and after the stall_timeout of 0.5s every action automatically aborts. Actual jitter that I see in gazebo is around 1e-7, so setting this to 1e-6 fixed this. I'll have to test a bit

more during grasping with this, but the original 0.02 is way too high for what I see.

Now actions still abort, but after the default timeout set in moveit as you suspected. When I give an explicit timepoint in the pre_grasp_posture everything works fine.

The lifting starting immediately is caused by the implementation of pr2_moveit_controller_manager:

    // the gripper action reports failure when closing the gripper and an object is inside
    if (state == actionlib::SimpleClientGoalState::ABORTED && closing_)
      finishControllerExecution(actionlib::SimpleClientGoalState::SUCCEEDED);
    else
      finishControllerExecution(state);

With the wrong stall velocity this immediate abort caused the assumption that the object is now grasped.

An aborted goal when grasping succeeds is something I find odd from a _grasping_ controller. Not very clear semantics. The grasping controller I use allows for two kinds of objectives: position and effort, so a goal is considered successful when _all_ joints have reached either objective, otherwise it fails. It thus allows to discriminate the situations you are observing.

Best,

Adolfo.

[Syed Ali Raza]

Hi Christian,

I am also following the same tutorial and having the same problem mentioned in this post. Can you please show how exactly you are explicitly setting timepoint for pre_grasp_posture?

Regards,
Ali

[Christian Dornhege]

As for the gripper controller: My guess is that unless the touch sensors are mounted there is just not other way to determine if/when an object is grasped as stalling the gripper looks the same like stalling due to actually grasping an object.

This seemed unexpected for me too, which is why I dug until I found that code.

The timepoint for the postures can be set when creating the goal for the Pick action to move_group, e.g.

  std::vector<moveit_msgs::Grasp> grasps;
  moveit_msgs::Grasp g;

  g.grasp_pose = p;

  g.pre_grasp_approach.direction.vector.x = 1.0;
  g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
  g.pre_grasp_approach.min_distance = 0.2;
  g.pre_grasp_approach.desired_distance = 0.4;

  g.post_grasp_retreat.direction.header.frame_id = "base_footprint";
  g.post_grasp_retreat.direction.vector.z = 1.0;
  g.post_grasp_retreat.min_distance = 0.1;
  g.post_grasp_retreat.desired_distance = 0.25;

  g.pre_grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");
  g.pre_grasp_posture.points.resize(1);
  g.pre_grasp_posture.points[0].positions.resize(1);
  g.pre_grasp_posture.points[0].positions[0] = 1.0;
  g.pre_grasp_posture.points[0].time_from_start = ros::Duration(45.0);

  g.grasp_posture.joint_names.resize(1, "r_gripper_motor_screw_joint");
  g.grasp_posture.points.resize(1);
  g.grasp_posture.points[0].positions.resize(1);
  g.grasp_posture.points[0].positions[0] = 0;
  g.grasp_posture.points[0].time_from_start = ros::Duration(45.0);

  grasps.push_back(g);
  return group.pick(object, grasps);

Am Mittwoch, 14. Januar 2015 20:13:15 UTC+1 schrieb Christian Dornhege:

[Syed Ali Raza]

Hi Christian,

Thanks. In my case I have to set both, default 'stall_timeout' and 'time_from_start', to very high values in order to make it work. Now, the complete pick and place is working fine but the gripper's opening and closing are very slow. Is it also happening to you or anyone? And how to make it fast?

Regards,
Ali

[Adolfo Rodríguez Tsouroukdissian]

On Thu, Jan 22, 2015 at 8:20 AM, Syed Ali Raza <syedali...@gmail.com> wrote:

Hi Christian,

Thanks. In my case I have to set both, default 'stall_timeout' and 'time_from_start', to very high values in order to make it work. Now, the complete pick and place is working fine but the gripper's opening and closing are very slow. Is it also happening to you or anyone? And how to make it fast?

Hey Syed,

Please try to understand what those parameters mean, so you can choose their values meaningfully (and not arbitrarily).

The 'stall_timeout' applies to the pr2_gripper_action, and is documented here:

http://wiki.ros.org/pr2_gripper_action#Parameters-1

The 'time_from_start' parameter is part of the specification of a trajectory waypoint, and indicates the duration (since the trajectory start time) at which a waypoint must be reached. The message definition is here:

http://docs.ros.org/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html

I hope this gets you on track.

Adolfo.

Regards,
Ali

[Christian Dornhege]

Changing the stall_timeout was probably the wrong parameter to change. Now it never stalls and thus always timeouts (only).

You might want to adjust the stall_velocity_threshold and reduce that to a value that is just below the normal movement velocity of the gripper.

Am Mittwoch, 14. Januar 2015 20:13:15 UTC+1 schrieb Christian Dornhege:

[Syed Ali Raza]

Thanks. Now, I have set stall_velocity_threshold to a small value and stall_timeout to default value. I have also set time_from_start to 45. Now, It does complete both pick and place but still the gripper closes/opens very slowly.

Regards,
Ali

[Adolfo Rodríguez Tsouroukdissian]

On Fri, Jan 23, 2015 at 6:22 AM, Syed Ali Raza <syedali...@gmail.com> wrote:

Thanks. Now, I have set stall_velocity_threshold to a small value and stall_timeout to default value. I have also set time_from_start to 45. Now, It does complete both pick and place but still the gripper closes/opens very slowly.

You are specifying that a waypoint should be reached in 45 seconds. That sounds slow to me, have you tried making that value smaller?. 

Regards,
Ali

[Syed Ali Raza]

Yes, I have tried small values for time_from_start but gripper still opens very slowly. Also, it aborts before completion of pick action (for smaller values of time_from_start).