Join GitHub today
GitHub is home to over 28 million developers working together to host and review code, manage projects, and build software together.Sign up
Object attaches to palm_link, gripper keeps closing #1
Sorry for the long question.
Thanks in advance, Jennifer. Cheers.
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):
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.
Let me know how it goes, good luck.
Thanks Jennifer for your fast response.
I tried with
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 ros_control as you proposed (I've never used ros_control before).
I'll let you know!!
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:
(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.
Hello again, Jennifer.
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
All with the same results: Attach - gripper closes through object - object jumps - detach
Have you been able to reproduce this?
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.
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.
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!
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.
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?
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:
I indeed am using the standard pr2 model.
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.
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
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
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.
added a commit
Mar 7, 2016
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.
So what can you do?
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.
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:
That should do the trick. Not too hard.
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'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!.