EGPlanner running in infinite loop

[akash-greyorange]

Hello ,
I am adding some graspable object in graspit world along with my robot.I do have some obstacle over which I am placing this graspable object.Pose for this object is basically fetched through pose estimation running on my machine. I am getting following logs and it goes on infinitily printing it without planning in some cases.

created new L1 Norm GWS.
Neighbor gen loops: 11
Neighbor gen loops: 11
Neighbor gen loops: 11
Neighbor gen loops: 13
Neighbor gen loops: 11
Neighbor gen loops: 16
Neighbor gen loops: 12

Eigen Planner never comes out of this loop.What does this exactly mean?My problem gets resolved if I use space search as COMPLETE instead of Axis angle.My graspit world is cluttered consider a rack consisting of my objects between which my grasp object is placed.

[jvarley]

Is there anything similar to:
Robot::loadContactData - failed to read number of contacts

Being printed when you load the robot into graspit. My guess is that the virtual contacts on the hand (little red arrrows) are never loading. Then when you run the planner, the energy functions do not behave regularly because they all depend on the hand having virtual contacts.

I think that the reason this is happening is that you have an old version of the libgraspit.so library floating around that has an old version of the code to load the robot contacts. Can you do the following:

jvarley@skye:~$ ldd /usr/local/bin/graspit_simulator 
	linux-vdso.so.1 =>  (0x00007ffe15f96000)
	libgraspit.so => /usr/local/lib/libgraspit.so (0x00007f68cf07c000)

and verify that libgraspit.so is not pointing towards something in a ros workspace or anything other than /usr/local/lib/libgraspit.so.

[akash-greyorange]

Nothing of this type
Robot::loadContactData - failed to read number of contacts
is getting printed on my logs while loading robot.My robot gets loaded perfectly in graspit world.
I observed that this problem arrises if my gripper palm link length is too big and if I keep my gripper length normal it dosent occur also it gets resolved sometimes by choosing space search option to COMPLETE instead of Axis angle.Also if I extend length of my gripper and put the grasp object outside of bin of rack which is my actual environment to test it works perfectly and as soon as I place object inside bin problem of this Neighbor gen loops occurs.So inside cluttered environment with very big gripper Axis angle space search dosent work.

jvarley@skye:~$ ldd /usr/local/bin/graspit_simulator
linux-vdso.so.1 => (0x00007ffe15f96000)
libgraspit.so => /usr/local/lib/libgraspit.so (0x00007f68cf07c000)
Well for this section I tried ldd /usr/local/bin/graspit_simulator this command but I havent installed graspit simulator through source and used graspit ros interface provided over here https://github.com/JenniferBuehler/graspit-pkgs by Jeniffer.This repo uses some graspit code v2.2.0 forked in local branch of Jeniffer so I dont know exaclty how can I solve this problem. I dont have libgraspit.so in my /usr/local/bin/graspit_simulator

[akash-greyorange]

Another update, this Neighbor gen loops logs keep poping for first few second on every plan and suddenly start planning after some indefinite time which actually increases my planning time.So is updating to new graspit source only solution?

[jvarley]

If you are using Jennifer's work, just make sure you use her fork of graspit:

https://github.com/JenniferBuehler/graspit

and I would imagine they should be compatible.

It sounds like the environment is the problem. Can you post several screen shots in both the configurations where the planner runs fine, and in the cases where it does not run correctly?

[akash-greyorange]

Yes I am using the same fork of Jennifer which is compatible with ros.
I am attaching screenshots of all my scenes
In first screenshot it is working fine since grasp object is not in collision.It generates Neighbor gen loops logs but works after some time.

Second screenshot grasp object is in collision with obstacle and Neighbor gen loops logs keep on poping.

[jvarley]

I am thinking that your shelf maybe having an adverse effect on GraspIt!'s collision detection system, and it is having a hard time generating valid neighbor states that are collision free during planning.

The EGPlanner search state calls collision detection via:

https://github.com/graspit-simulator/graspit/blob/master/src/EGPlanner/energy/searchEnergy.cpp#L117
https://github.com/graspit-simulator/graspit/blob/master/src/world.cpp#L1470

Perhaps throw in some print statements or something and try to determine if it is failing to generate collision free poses.

[akash-greyorange]

Yes you were right I printed some debug logs and it says hand is colliding with world constantly at start of planner so mybe that's the reason for Neighbor gen loops.I actually changed Searchstate from Axis-Angle to Complete which results in good output since planner hangsup less times but still it hangsup in some cases.So what can be done to solve this issue?
Actually my gripper palm link is elongated which I did purposely to avoid poses grabing object from backside since those poses wont get executed by my arm.So is their any way to solve these issue of back poses other than extending my gripper length?And will gripper with normal length fetch good results as compared to one with elongated palm link?

[akash-greyorange]

Hi @jvarley I guess problem is with my gripper size.I tried to plan in cluttered environment with jaco hand where it didnt stopped for Neighbor gen loops and if I used my Robotiq hand with some custom ellongation added in order to avoid poses grabbing object from backside it takes time for starting the planner.Main motive behind extending hand long in size was just to avoid back poses which wont be achievable by my ARM so I need some work around to solve this issue.Is there something which can be done like checking if hand is transalated behind object position in world like my approach axis is X-axis so I can check weather my hand is beyond my object and guide planner to stop looking for those poses and jump to approach object from front side which is hand position for x-axis less than object position for x-axis?

[akash-greyorange]

@jvarley Any help regarding my scenerio?Any updates?In short I want to define planner region in which it shoud look for poses.This region should be cone around object to be grasped.I know in my scenerio my objects will be placed in x-axis in world frame so I can put check in planner poses to discard poses behind my object pose in x-axis and then I also want to reject poses holding object from upside which means pitch of my hand shouldnt be more then certain limit.I need to put these 2 checks in planner main algorithm and perform planning inside these limits.Will this be possible by some modification in graspit code?

[jvarley]

I would modify the guided potential quality energy and add additional terms to the energy function to penalize grasps that are outside limits you set. Just make sure you penalize it in a way that has clear downhill gradients, so that hand is able to move to better positions. I.E. larger penalties for the farther the pitch is beyond your limit not a constant error term for being beyond your pitch limit.

https://github.com/graspit-simulator/graspit/blob/master/src/EGPlanner/energy/guidedPotentialQualityEnergy.cpp#L11

[akash-greyorange]

Thanks for the reply @jvarley . I got first part of your solution that to pass limit arguments to guided potential quality energy function and check for limits in that function but how to do the second part which is to move hand to better positions or position within my limits?Also how to assign penalties to pose which is way out of my limit?

[akash-greyorange]

@jvarley According to my knowledge after going through whole flow of code my initial guess was to penalize grasps before marking it as legal in function analyzestate of SearchEnergy.cpp and if I penalize grasp after marking them legal and storing energy will that suffice my need? Instead if I penalize grasps at
https://github.com/graspit-simulator/graspit/blob/master/src/EGPlanner/energy/searchEnergy.cpp#L173
Will it work that way?Correct me if I am wrong

[jvarley]

If hand states that you don't want have high energy, the annealing process will naturally avoid them as it moves to better low energy states.

Your idea of simply making these states illegal would probably work as well. I would say try it. The only possible issue I see if that if the planner starts in an illegal state and a large fraction of the search space is illegal, then it may have a hard time getting itself to a legal state since there is no gradient for it to follow, while if you give it a really high energy, for really bad grasps and just a high energy for bad grasps, then the annealing process can move downhill to the regions you want the planner to spend its time in.

[akash-greyorange]

Yes you were right I tried making states illegal directly in analyzestate function which stops planner from finding better pose and code gets stuck for indefinite time.I did as you said penalizing energy with some high error which depends upon error between my legal state and currently illegal state which works perfectly and gives me pose which are inside my desired region.Thanks alot for guiding me in proper direction. Sometimes Planner takes time to initiate planning so is that because planner being in illegal state or collision with environment can be problem?I have made my hand short as compared to previous so I dont think so its collision issue.So if its illegal state issue at start should I manually place hand in legal state and then start planner for quick results?

[jvarley]

Glad things are working for you. My guess is that would help, but try it. You could potentially put the hand in a good position and resave your world file, then it will just start in that position everytime you start graspit.

[akash-greyorange]

Yup Thanks for all the help @jvarley . It happens in very very rare case that hand comes in illegal state and dosent come out of it due to constant energy or it is unable to move then maybe I should detect its situation and manually move robot to known legal state.Well will find out some solution out of this.

[akash-greyorange]

@jvarley I had one more question.Is it possible to import PCD(Point cloud) file directly to Graspit environment.I havent seen it yet and currently thinking about converting PCD to iv and then importing it as obstacle.Are there any other ways to do this?

[jvarley]

Some really old code for putting a pointcloud into the graspit scene:
https://github.com/CURG-archive/graspit_bci/blob/cmakify/src/Servers/graspitServer.cpp#L1192

The above code was for visualizing pointclouds sent over a socket connection to graspit. It could be modified to load from a pcd. I have not ever used pointclouds as collision objects within graspit, only for visualization. It is really designed to work with meshes and I would not recommend doing anything with pointclouds in graspit. I would mesh your cloud, then plan on the mesh. But nothing says its impossible, so good luck.

[akash-greyorange]

Ok will mesh my point cloud and work with it. Thanks @jvarley for all the help.We can close this issue now. :)

[akash-greyorange]

@jvarley One more question.Is there any specific steps to learn grasps produced from planner and save them like CGDB is doing but it has grasps for Barret hand and human hand.I need to create some learning based model which will save all the possible grasps for my object but also there are certain constraints like my environment is dynamic consider rack picking as it was in APC last year for which I will be adding rest of the scene pcd in form of mesh in my planning scene with my grasp object kept seperately.I need to create database of grasps for my robotiq hand.Can you refer some guidelines for doing the same?

[jvarley]

Call graspit from graspit_commander:
https://github.com/graspit-simulator/graspit_commander
Generate a bunch of grasps, and dump the ones you like into a pickle file.

[akash-greyorange]

Ok Thanks :)

[akash-greyorange]

Is there any example for connecting this grasp database to main graspit planner via cpp interface so that it will not plan and just give me best grasp from database?Also my assumption for using this grasp database is like in any kind of cluttered environment I will get top good grasps within my defined region which are not in collision considering various poses of grasp object.Am I right?Or I will have to do collision checking explicitily after getting grasp poses from database?I mean how collision checking is handled while dealing with DB

[akash-greyorange]

One more query. I downloaded and loaded CGDB in my database postgres and also set CGDB environment variable to point to psb objects.Now if I go to connect and browse database from UI then it gives me segmentation fault without giving any reason.I followed steps in graspit manual http://graspit-simulator.github.io/build/html/cgdb.html and I couldnt find graspit.pro file.If you have any updated steps to connect to CGDB kindly send me the link for the same.

[akash-greyorange]

@jvarley I wanted one more help.Since object pose estimation is major requirement to plan stable grasp can you guide me where to look for some stable pose estimation library?

[jvarley]

https://github.com/tum-mvp/ObjRecRANSAC
with ROS:
https://github.com/tum-mvp/mvp_objrec

[akash-greyorange]

Cool will have a look into this.Thanks :)

[samarth-robo]

For anyone experiencing this issue, the cause, as @jvarley said in a comment before, is "it is having a hard time generating valid neighbor states that are collision free during planning".
The Simulated Annealing planner works by generating neighbour states from the current state, and picking one of them according to the temperature.
But if the robot is in a state such that all neighbour states have collisions with some obstacle (these are considered invalid in GraspIt!), then this process gets stuck.
So to solve this, change your robot pose to be a little away from obstacles, so that atleast one neighbour state is collision-free.