Large numbers of always-active part contacts on tray cause slowdowns

[Steven Gray]

I can see that the part on the tray is never allowed to go inactive, physics engine-wise.
There are a ton of contacts and just one gasket on a tray slows the sim from 0.98x to 0.52x on my machine. Add a few more onto a tray and we're down to 0.2x.

The parts can go inactive on the bins and floor -- gazebo doesn't show contacts after a second or so.

My experiment:

• Startup: 0.98x RTF
• Dropping a part (gasket part) on the ground causes the RTF to drop to 0.54x for a second, then come back up to 0.98x -- assume
• Put that same gasket on the conveyor belt and RTF down to 0.86x permanently
• Same gasket on tray 2 results in sustained 0.52x RTF

[Steven Gray]

This is similar to #32. The RTF numbers above are without gazebo logging, but I suspect this could really be an issue with that enabled.

@sloretz This is also why I assumed that the docker image was somehow resource-limited in #163

[Steven Gray]

This is weirder than I thought, as just moving stuff around in gazebo using the UI I've gotten a couple gaskets that never stop the collision checks, even on the ground or bins. There are a lot of contacts flickering on and off.

All other parts stay active on the tray, and also on the ground after being on the tray. They have far fewer contacts, and those contacts are stable, so this is less of an issue.

[Rud Merriam]

This is also dependent on your code. I noticed Gazebo slowing down at times and realized my code was spinning its wheels waiting for arm movements to finish. I added some Durations in that loop and Gazebo ran faster.

[Steven Gray]

True, but I wasn't running anything except the osrf_gear qual_a_1 without logging when doing this test, moving things around with the Gazebo UI.

[Steven Gray]

Long story short:

• Using the gasket mesh for collision rather than the current large number of simple shapes makes the sim quite a bit faster (from 0.45x to 0.77x for me with a single gasket on a tray). This is a quick fix (PR #140)
• All objects that come in contact with a tray have model->SetAutoDisable(false); called on them and this is only undone when LockContactingModels() is called during shipment submission. Anything else that touches a tray (if it's a faulty part or one we put back because the order was preempted/updated) remains active until it gets submitted as part of a different shipment or the sim ends. (PR #141)
• The kit tray plugin runs at 20Hz, but only updates at 1Hz as that's when there's a new collision message from gazebo on /gazebo/default/agv1/kit_tray_1/kit_tray_1/tray/kit_tray_contact (updates at 1Hz), so keeping these contacts active at 1kHz seems unnecessary.

[Steven Gray]

@sloretz After PR #140 and PR #141, the biggest remaining speedup would be to change things so it wasn't necessary to keep contacts active on the tray (there's really no way around this for the belt). Keeping them active seems only to be needed so that the KitTrayPlugin can clear contactingLinks and contactingModels each time it runs (which is just at 1Hz). Can we make this list persistent, just like fixedJoints is? Then we only have to store contacts with the tray when they are new.

[Shane Loretz]

but only updates at 1Hz as that's when there's a new collision message from gazebo on /gazebo/default/agv1/kit_tray_1/kit_tray_1/tray/kit_tray_contact (updates at 1Hz), so keeping these contacts active at 1kHz seems unnecessary.

Unfortunately the parts stay active if AutoDisable is enabled between kit tray updates. The threshold is 1 sim second.

Can we make this list persistent, just like fixedJoints is? Then we only have to store contacts with the tray when they are new.

The problem to solve is how to figure out the difference between a part no longer being in contact with the tray and the part disabling due to no movement. Maybe the world pose of the part could be compared to see if it moved?

[Steven Gray]

I like it -- storing the world pose of the part every time contact is registered, then comparing against that.