Difficulty in picking up parts (Specially Pump)
AravindaDP opened this issue · 13 comments
It is bit harder to pickup pump part since it seems collisions forces from both side of part from bin and robot causes jittering of part in the simulation environment before vacum_gripper_plugin detects the part contact and creates the link between part and robot arm.
While it is not clear whether it has any effect, it seems that there has been a plugin called ModelContactPlugin in ariac2022 which seems not used in this year.
As I understand it, it's a bit tricky simulation to pick parts without disturbing them in this manner. (Probably we need a mechanism to disable physics effect on robot - part collision while still being able to detect collision to detect contacts and attach the part (perhaps using a filter plugin to disable selected collisions being used by physics engine). Or need other mechanism to detect proximity of gripper and part before actual collision occur and based on it need to attach part)
We recognize that it can be difficult to pick up parts due to the constraints of the simulation. The implementation with the test competitor seemed fairly robust in my testing even for the pump. Have you looked at that implementation for reference?
As for the ModelContactPlugin the main code used in the plugin has just been integrated into the VacuumGripperPlugin but the approach is the same as previous years.
Update: I created a new branch here called pump_model_updates and changed the sdf file for the pump to better match the setup for other parts. Can you test your code with the new trial four_pumps.yaml in this branch and see if this configuration works better with your system?
Yes actually I'm using the test_competitor as is as the base class for low level robot handling with our solution more focusing on high level logic to cater for agility.
I can confirm that pump_model_updates branch worked better than version1.5_staging or ariac2023. (I use version1.5_staging even though it's still not merged yet, Should work same as ariac2023 for the scope of this issue.)
When using version1.5_staging it failed on first pick for 3 consecutive trial runs. It caused the pump to strutter, slip out of robot and kept rotating and disturbing all other pumps.
When using pump_model_updates it worked flawlessly for 4 trials out of 6 (With all 4 pumps properly picking) But on 2 trials, one out of 4 pumps caused a mishap at the drop off on the tray. (Which I can live with as long as at the finals, we can request for a rerun if it found simulation issue cased a mishap like these)
What kind of mishap were you seeing?
drop_issue.mp4
It's not clear if it's caused by imbalanced residual force from a contact at the time link is removed on the drop time or planner planned a path where part hits the tray before drop off. In any case with new update this only occurs randomly and rarely (But with like 5% or less chance)
I think the issue here might be that the part is being dropped too low and it is interfering. I would try to increase the offset that the part is dropped onto the tray and see if the issue still exists.
It is very frustrating that certain conditions cause the pump part causes issues when picking back.
Here the part is flipped. Note that it works OK when picking from the bin. Also picking back from agv when it is not flipped also work OK.
Issue do not occur for flipped battery also. (When picking from bin or picking back from agv)
pick_back_flipped.mp4
To clarify the issue is with re-grabbing a pump from the AGV when it is flipped? Are you using the code from the test competitor for grabbing the pump? As a reminder the test competitor is not able to handle flipped parts and does not account for it as a possibility.
While I'm using the code from test competitor, following additional changes were made to handle the flipped parts.
- To pick from bin, z position of part pose retrieved from logical camera was changed back to the same level as bin surface (Since on flipped parts, link is on the top surface of the part opposed to bottom face) This enabled to rest of the FloorRobotPickBinPart method to work with flipped parts.
- Currently I'm attempting to place flipped parts as is (flipped) on kit tray, then after QC service called, pick parts indicated as flipped on the tray using floor robot and hand it to ceiling robot to place on correct pose. This is where things go haywire.
- For this pick from kit tray operation I modified FloorRobotPickBinPart logic by using agv_tray_pose + quad_offset as the position of part. This works for parts not flipped. Only pump when flipped is showing above issue in this scenario.
- Logical change I have done above should theoretically not cause any issue (This assumes that parts lie flat on the surfaces and everything lie fairly perfectly horizontal orientation. The numerical rounding off errors in simulation should theoretically not cause the issue. But in practice it seems how the vacuum gripper is implemented and these numerical rounding off errors cases pump part to get attached to robot hand at a rotated angle that causes a sudden collision force between tray-part-arm that causes a jerk force causing everything to break hell.
On our testing this caused issues with following scenarios in finals files from 769c043
final11_assembly - FINAL11ASSEMBLY1 task works perfect (Even though it has a pump). Picking Pump for FINAL11ASSEMBLY2 breaks the simulation. No apparent bug on competitor code that could blame on (Since both tasks are perfectly identical it seems and handles similar to each other)
I can give the assembly trials another try if you get different results.
When it's causing problems it pretty much consistently causes problems. May be When AGV is left side of the assembly station it may be causing problems. Did following tests.
trail 1 and 2 - final11_assembly as is. Both attempts picking blue pump for 2nd task causes havoc
trial 3 - Changed order of announcements (task1 with 50 sec delay and task2 announced at the beginning) Failed immediately on task2 (Which is now the first to be announced at the beginning) blue pump pick
trial4 - repeat of trial3 configuration. both tasks works fine giving perfect score
trial 5 and 6 - Again fianl11_assembly task as in the original fails at task2 (Now announced with a delay of 50 sec)
So it's a pretty much random chance it seems. May be left side AGV position causes a singularity situation in orientation solvers on either simulation or MoveIt. (I guess it should be re-creatable with test_competitor assuming it's capable of handling all assembly stations)