Object attaches to palm_link, gripper keeps closing

[mexomagno]

Sorry for the long question. First of all, thanks for the wonderful plugin. I'm currently using it with a _PR2 _on ROS Hydro, within Gazebo 1.9. It compiles smoothly, runs with no problem, and when the gripper starts closing around the tested model (wooden cube), when it completely encloses it, the plugin successfully attaches it to the palm link specified on the model's urdf (technically, xacro). But the gripper won't stop closing, trespassing the object's geometrical boundaries, and after a certain point, the plugin will detect a collision and proceed to detach it, making the cube jump. It then enters on a detach-attach loop until the object jumps out of reach. The gripper then closes completely as there's nothing avoiding it. I'm aware that the following is ROS and MoveIt specific, but I hope someone can help anyway. The way I'm closing the gripper is somehow forced: I'm not using Moveit to do it (I don't know how!), I'm directly publishing a pr2_controllers_msgs/Pr2GripperCommandActionGoal msg to the /l_gripper_controller/gripper_action/goal topic. Can this be the problem? It is of my understanding that the pr2 gripper will always keep trying to close when it encounters an object in between the fingers, applying a given effort specified on one of the msg fields, so this is an expected behaviour. I wasn't able to find the source files of the Jaco example shown on your (excellent) Wiki.

Thanks in advance, Jennifer. Cheers.

[JenniferBuehler]

Hi Max,

The plugin only attaches the object to the robot when there are two opposing forces applied by the grippers. It does not prevent the grippers from further moving. That has to be done outside the plugin, as you recognized correctly.

If I understand this correctly, your grippers are moving "through" the object? This should not happen if physics is working correctly. I would have to look it up again, but I believe that contact points (used by the plugin) should only be generated if collision is enabled. So if the plugin is working, it means collisions are enabled. But you could check on that, just to be sure. The grippers should not move through any object with collisions enabled in both gripper and object, no matter which action command or controller you are using. The robot rather explodes before it moves through collision objects ;) It may be possible however that such a huge force is applied, that there's some weird things happening with the physics. You could try to use ros_control with the joint_state_publisher to manually close the grippers and see if it keeps happening.

In Gazebo, and I think particularly in older versions as 1.9, it is important to have physics run with a high iteration count when grasping. I haven't finished this wiki page yet but it may help you anyway. There is also a service "test tool" which you can use to set the physics properties after Gazebo has been launched (do a new "git pull" as I've just moved this from the jaco-arm-pkgs):
rosrun gazebo_test_tools set_gazebo_physics_client

Next thing to check would be the material properties of the object you are trying to grasp (the PR2 is probably ok if you're using the standard model). Are you using the cube spawned by the cube spawner? With this cube it should be ok I think, because it works with the Jaco.

The Jaco packages are here. Just add them to your catkin_ws. You can follow this tutorial to try the plugin on the Jaco. Do you get the same problem there?

Let me know how it goes, good luck.

[JenniferBuehler]

Note that for the new version of gazebo-pkgs, you will also need convenience-pkgs (previously called tools-pkgs), as described on this wiki.

The frequent changes in the packages are because I'm preparing to release them as official ROS packages.

[mexomagno]

Thanks Jennifer for your fast response. As you can see here, I meant exactly moving through the object. I tested this with different objects available from the gazebo interface ("insert" tab), which is where I got the wooden cube you can see on the previous video (and not from the cube spawner). The collisions seem to work well on all objects until just before the plugin attaches the object to the palm link. I had gazebo physics settings set to values very similar to those on your wiki page, and just to be sure, I configured the world with the exact same values you propose. I can confirm that I'm loading the right world file by calling /gazebo/get_physics_properties, which yields the following:

time_step: 0.001
pause: False
max_update_rate: 1000.0
gravity: 
  x: 0.0
  y: 0.0
  z: -9.8
ode_config: 
  auto_disable_bodies: False
  sor_pgs_precon_iters: 0
  sor_pgs_iters: 50
  sor_pgs_w: 1.3
  sor_pgs_rms_error_tol: 0.0
  contact_surface_layer: 0.001
  contact_max_correcting_vel: 100.0
  cfm: 0.0
  erp: 0.2
  max_contacts: 20

I tried with sor_pgs_iters: 100 (<iters>100</iters> on the .world file) too, without success. Should I keep raising this number? I also tried with low values of effort with the same results. The only thing that changed with this is that the gripper closed much slower, but still inserts into the object.

Tomorrow I'll definitely try to run the Jaco files to see if I'm still having this problem there, and I'll let you know. Also, I should try using _roscontrol as you proposed (I've never used ros_control before). As for the new version of gazebo-pkgs, I just installed the dependencies and cloned them. I'm having some trouble compiling it (on SetGazeboPhysicsClient.cpp had to change all the float to double or else it would fail!) and will look into it tomorrow too. I'm using your old version of the plugin right now (which I think hasn't changed).

I'll let you know!!

[JenniferBuehler]

Nice video! :-)

It's strange indeed, it seems like it's getting just across a small space of collision geometry and then detaching the object immediately again (as it would be expected if there's no contacts). I'll try in the next days to reproduce the error, maybe it happens on Indigo/Jade as well. What's the name of the cube (of the gazebo objects library) you're using?

The iterations should be more like 500, but I don't think this is the issue here. If iterations are too low, from what I know you'd rather get something like the robot exploding, or the wobbling problem you described in your earlier email.

Hmm next I would maybe try another object. Maybe the one you're using doesn't have a proper collision geometry, though it seems it has something going collision-wise (otherwise you wouldn't have had the wobbling either). It would be worth trying another object anyway, just to make sure. You could for example spawn my example cube, which works for the Jaco:

rosrun gazebo_test_tools cube_spawner <any-name> <x> <y> <z> <relative_link>

(it's in the gazebo-pkgs as well so you should have it already). You can for example specify the table as relative_link and then find a good place to spawn it.

If I can reproduce the issue, maybe I can improve the plugin so that it prevents this from happening. Thanks for reporting this issue.

[JenniferBuehler]

Oh I just saw that I have 50 as iterations on my wiki page. I'll look tomorrow again to be sure, but I think I was using 500 on Hydro. This must be a mistake on the wiki.

[mexomagno]

Hello again, Jennifer. I tried using 500 iters and apart from making the simulation unbearably slow, it didn't fix this problem :( I'm getting kinda desperate. Trying to compile your code yields the following error:

[ 98%] Built target fake_object_recognizer_node
Linking CXX shared library /home/mexomagno/catkin_ws/devel/lib/libgazebo_world_plugin_loader.so
/usr/bin/ld: cannot find -lDepthCameraPlugin
/usr/bin/ld: cannot find -lGpuRayPlugin
/usr/bin/ld: cannot find -lRayPlugin
/usr/bin/ld: cannot find -lContactPlugin
/usr/bin/ld: cannot find -lCameraPlugin
collect2: ld devolvió el estado de salida 1
make[2]: *** [/home/mexomagno/catkin_ws/devel/lib/libgazebo_world_plugin_loader.so] Error 1
make[1]: *** [gazebo-pkgs/gazebo_world_plugin_loader/CMakeFiles/gazebo_world_plugin_loader.dir/all] Error 2

I don't know how to fix this. Anyway, I adapted your cube spawner to make it work. The blue cube showed the same behavior.

The exact model name of the cube I used is wood_cube_7_5cm. So far I've tried with:

• beer
• cricket_ball
• wood_cube_7_5cm
• lumber_2x4x48
• plastic_cup
• tube_2_25cm

All with the same results: Attach - gripper closes through object - object jumps - detach

Have you been able to reproduce this?

[JenniferBuehler]

Hi Max,

poor you, yes I know such desperate moments too well ;) sometimes things take ages, that's a programmers life ;)

The linker error is because it doesn't find the Gazebo plugins. Can you please send the output of locate libDepthCameraPlugin Try adding the path to $GAZEBO_PLUGIN_PATH. If you don't get results for the locate command, you may need to install a gazebo library package, I can find out which one it is (maybe gazebo-plugins or similar name, look in your package manager for something like that). If you end up finding it, let me know so I can add it to the package dependencies (I don't need them on Indigo or Jade but would add it to the documentation anyway).

I haven't looked into reproducing your error yet because there's something else with the grasp execution keeping me super busy (speak of long nights) and I was waiting to hear back from you. I will look at it tomorrow, promise.

If it also happens with the blue cube then the issue is not the object. My next guess is that it's something with the PR2, as the same plugin with blue cube was also working for me in Hydro with the Jaco back then.

[mexomagno]

Haha, well, I've no other option but to keep trying as I just have about 2 weeks left to do this. If the gripper grips... I'll be finally able to test my placing algorythm. But in Jennifer we trust!

[mexomagno]$ locate libDepthCameraPlugin
/usr/lib/gazebo-1.9/plugins/libDepthCameraPlugin.so
[mexomagno]$ echo $GAZEBO_PLUGIN_PATH
/usr/lib/gazebo-1.9/plugins

Maybe you could share the exact same PR2 model you were using back then?

Maybe there's a simple way of stopping the gripper movement at the exact moment when the Attach occurs? That would be enough for my use case, assuming that it will be able to release the object after.

[JenniferBuehler]

I wasn't using the PR2, I was using the Jaco... therefore I'm guessing it's a property in the PR2 grippers (not that I think it's a bad model at all, maybe just has some properties which require special handling for the plugin). I can try on Indigo / Jade with the PR2 as well and see if it happens. Guess you are using the standard model? You don't have a repository with your launch files by chance so I could try the exact same thing?

Actually, I will be adding a publisher to notify any kind of nodes about a grasp having happened in the next days. This could help you implement the patch you suggested.

About the linking problem: weird, it's as it should be. You can try to add this line to the CMakeLists: link_directories(/usr/lib/gazebo-1.9/plugins) but I'm guessing now that you fixed it by other means it's not of immediate priority any more, especially because it's not a problem with the objects anyway.

[mexomagno]

I indeed am using the standard pr2 model. I do have a repo... but it's messy. I just pushed the changes I've made so far since I started to test your plugin, so you can have a look if you want. I know you don't have to, but if you do, I hope I can help you with anything you need. The main packages are memoria, memoria2 and mipr2_gazebo. I start everything by doing:

$ roscore & roslaunch mipr2_gazebo pr2_moveit_mesa_cubo_fixed.launch

The publisher could be of help (but wouldn't your plugin stop being ROS independent?).

I must say, its totally possible that I have some misconfigured files or something like that. I painfully learned to use all these tools (ROS, gazebo, pcl lib, the actual PR2, etc) by myself in a short period of time and in various moments of struggle I edited here and there.

Many thanks.

[JenniferBuehler]

Thanks for sharing the repo. Don't worry about the "messy" thing, I know very well how code looks when done under time pressure and working with something new ;) Good on you for doing it. Your video looked awesome, may I ask which screen capture tool you were using?

I haven't looked at it yet, but by the sounds of it, it could not be so difficult to start up your stuff, and it will give me the chance to start debugging the plugin right away.

I have only a few days until I have to finish a bucket load of work (though the motivation is good because I'm going on a holiday). So I can't promise I get the time to look at it, it's pretty crazy in the last days.

I have a spontaneous idea though, it could be the 180 degrees between the PR2 grippers. The plugin checks whether opposing forces are applied, but if they're exactly 180 degrees they may be considered the same (which could happen frequently, but not always, in the PR2 and it would explain the behaviour of switching between attachment states). This would be a very stupid classic bug when comparing angles, but it would be easy to fix. I'll take a look in the morning. It's bed time here now.

Yes the plugin would then be ROS dependent, which is why it will be a subclass of the existing plugin, in another package. It will only activate the plugin if a grasp is currently intended, which gives it more control. There will be a message published to signal when a grasp is being done. You can subscribe to the same topic and then apply your fix.

PS: By the way, just as an info you may not have discovered yet: you don't need roscore before roslaunch, roslaunch starts roscore if it's not already running.

[mexomagno]

The video recording tool is Kazam. I further compressed the video with the one and only ffmpeg (now avconv).

I hope your holiday comes soon! I will miss the 2.5 month long holidays they gave us here at the University.

In this whole time I actually never figured out that roslaunch starts roscore too... lol, I could've saved a couple thousand key presses by now. Thanks!

Lets hope your spontaneous guess leads to something beautiful tomorrow. If not, no worries of course!

As always, thanks for your quick and awesome replies. Good night, Jennifer, and have a great week!

[JenniferBuehler]

Ok, so luckily I didn't make the stupid mistake with the angle comparison (I wasn't using cross product which sometimes is better if you need to know the direction of the angle).

I've spent some time testing this today. I have good news and bad news. The good news: It's now clear what the problem was. Stupidly, after testing many other things, I forgot to take a look at the basic attachment function which fixes the object to the link. In the end I discovered that I had disabled collisions in there. But why? That's the bad news. The bad news is that I did this back then because the wobbling still occurred to some extent even when the object was fixed to the link. But attachment is still required so the object stays in the robot hand. So I had to disable collisions. I had completely forgotten about this. So this is what your problem was: because you keep applying an effort (which I wasn't, I was just setting the joints to a grasp state very slightly "inside" the object), the gripper moves too far into the object, and then at some point there's no collision contact points with the object and the grippers (at least none that satisfy the "opposing forces" --- the object gets detached, collisions re-enabled, and then the gripper jumps back outside the object. That's what's been happening.

So what can you do? I've added a number of new parameters to offer more control, see the updated wiki. You should add the new parameters to the URDF. Then, try if you get back the wobbling with <disable_collisions_on_attach> set on false. If you still get the wobbling, set it back to true to get it back to how it was working before (you'd get your same old issue). Then, you could try to increase the value of <release_tolerance>, but don't set it too high, otherwise the object will remain attached even if the grippers have opened (see also description on wiki). This will allow the gripper to move into the object, however you should not apply your effort for too long, otherwise it will look weird. It will be ok though if the gripper only moves into the object a little bit.

Last note (phew this is getting long): About my comment late last night about the extension of the current plugin. A ROS message will be sent around when a grasp has been successfully executed. But that would only happen once the grasp is finished, and only if you were using my grasp action server, which you aren't. So you can't actually use it to stop your grippers. It was late last night and I didn't think of that. However it would be possible to add another extension of the plugin, which is also ROS dependent, and sends out that message from within the plugin just after the object has been attached. I won't be able to do this in the next days however I think, I have to catch up with my other work before I go away. But I can (and most likely will) do this in about 4 weeks time, which is not soon enough for you...

Hope you can fix your issue with what I suggested above. Let me know how it goes, maybe I have other ideas.

Cheers

[JenniferBuehler]

P.S. you have to do a fresh "git pull" of course as well

[JenniferBuehler]

I just added another small extension: The plugin class has not methods OnAttach() and OnDetach(). If my suggestions don't solve your problem, you can derive from GazeboGraspFix and implement these functions. In there, you can stop your gripper. This is what I'll do in 4 weeks but you could also do it:

• make a new catkin package which only creates one library and depends on gazebo_grasp_plugin. Name the library different to gazebo_grasp_fix.
• add the header/cpp file which derives from GazeboGraspFix and implements OnAttach() and OnDetach() with your fixes.
• In the URDF, specify your library name instead libgazebo_grasp_fix.so

That should do the trick. Not too hard.

[mexomagno]

Dear Jenny:

You're now officially my hero. The last modifications you did to your plugin made it work exactly as expected. I'm even using just 50 iterations and no important wobble is present! I can now perfectly grasp the wooden cube, and even the most difficult object yet: The cricket ball (out of frame).

I can't thank you enough, I would've never been able to come up with something like this plugin on time. And I thank you again for your outstandingly good disposition towards my problem.

I can now begin actually testing my algorithm, make some corrections, and complete the document (which must be ready by march the 28th). You'll be definitely mentioned on this report :)

I hope you have a great time on your holiday, and well, talk to you soon! (hopefully with good news)

I hereby declare this issue as officially closed!.

[JenniferBuehler]

Dear Max,

I'm very happy this solved your issue. Thanks also for reporting this, it made me improve the plugin where it needed some extensions.

Good luck with your paper!

Jenny