Optimize FCL move shape #3601
Optimize FCL move shape #3601
Conversation
|
Codecov ReportAttention: Patch coverage is
❗ Your organization needs to install the Codecov GitHub app to enable full functionality. Additional details and impacted files@@ Coverage Diff @@
## master #3601 +/- ##
==========================================
+ Coverage 47.15% 47.19% +0.05%
==========================================
Files 603 603
Lines 58999 59187 +188
Branches 6969 7026 +57
==========================================
+ Hits 27814 27929 +115
- Misses 30773 30840 +67
- Partials 412 418 +6 ☔ View full report in Codecov by Sentry. 🚀 New features to boost your workflow:
|
f53543c to
bf94ada
Compare
|
I will invert the logic and try to add tests. Do you know if the failing test is failing due to these changes ? [ FAILED ] FCLCollisionCheckPanda/CollisionDetectorPandaTest/0.RobotWorldCollision_1, where TypeParam = collision_detection::CollisionDetectorAllocatorFCL (13 ms) |
Update the FCL collision objects transform when a World::MOVE_SHAPE is received instead of recreating the object. Reduces significally the time to update the shapes, especially for large meshes.
|
My changes affect's one of the tests. Will investigate. |
bf94ada to
f8be3a5
Compare
|
@simonschmeisser Ok, this was not intuitive ! We got to recompute the AABB and unregister/register the FCL object from the broad phase manager so that the collision detector works. Added 2 more tests where the object is not in collision.
|
|
It now takes an average of 1.5 milliseconds to update the 3 meshes. |
| it->second.collision_objects_[i]->computeAABB(); | ||
| } | ||
|
|
||
| // Reregister the FCL object into the broad phase collision manager |
There was a problem hiding this comment.
Ideally the comment would explain why this needs to happen. Is there no API for moving objects in fcl? (I know it's not implemented for the manager actually used)
There was a problem hiding this comment.
I will try to see why this is needed (Actually I just repeated what was done here).
I can't find any API to move the object in FCL, only the CollisionEnvFCL::updateFCLObject which reconstruct's the object...
There is 3 4 options to update the broadphase collision manager:
std::vector<fcl::CollisionObjectd*> fcl_objects{it->second.collision_objects_.size()};
for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i)
{
it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i]));
// compute AABB, order matters
it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB();
it->second.collision_objects_[i]->computeAABB();
fcl_objects[i] = it->second.collision_objects_[i].get();
// Option #1 (18 usec)
// manager_->update(it->second.collision_objects_[i].get());
}
// Option #2 (15 usec)
// manager_->update();
// Option #3 (13 usec)
manager_->update(fcl_objects);
// Option #4 (7 usec)
// it->second.unregisterFrom(manager_.get());
// it->second.registerTo(manager_.get());
The fastest way seems to be the original fix I proposed.
f8be3a5 to
1be7c59
Compare
|
Did some benchmarks:
* Primitive Shape: World::moveObject This is not representative of a real world example, because the scene would have many more collision objects (meshes, primitives or planes). With 3 meshes we see that actually the By checking the the FCL Broadphase Dynamic AABB we can see the 3 methods. Method 1: registerObject/unregiterObjectThe I would argue that these method should be used when we only move shapes, because it will be inserted at the same place. Thus the dtree does not need to be rebalanced (See Method#2) ? void DynamicAABBTreeCollisionManager<S>::registerObject(CollisionObject<S>* obj)
{
DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
table[obj] = node;
}
template <typename S>
void DynamicAABBTreeCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
{
DynamicAABBNode* node = table[obj];
table.erase(obj);
dtree.remove(node);
}Method 2: updateWe iterate over every elements of the table to get the AABB. After this, the dtree is rebalanced. You can see here and here that this method was maybe used but has been commented in the code in favor of the register method. Another guess for it not being used is that the dtree update changes the structure of the tree even if the bounding volume has not been changed flexible-collision-library/fcl#368. It is called by template <typename S>
FCL_EXPORT
void DynamicAABBTreeCollisionManager<S>::setup()
{
if(!setup_)
{
int num = dtree.size();
if(num == 0)
{
setup_ = true;
return;
}
int height = dtree.getMaxHeight();
if(height - std::log((S)num) / std::log(2.0) < max_tree_nonbalanced_level)
dtree.balanceIncremental(tree_incremental_balance_pass);
else
dtree.balanceTopdown();
setup_ = true;
}
}
template <typename S>
void DynamicAABBTreeCollisionManager<S>::update()
{
for(auto it = table.cbegin(); it != table.cend(); ++it)
{
CollisionObject<S>* obj = it->first;
DynamicAABBNode* node = it->second;
node->bv = obj->getAABB();
}
dtree.refit();
setup_ = false;
setup();
}Method 3: update specific objectsWe iterate over only the specified collision objects of the table. After this, the dtree is rebalanced. I would recommend this method when adding a new collision object because the dtree should be rebalanced at some point ? void DynamicAABBTreeCollisionManager<S>::update_(CollisionObject<S>* updated_obj)
{
const auto it = table.find(updated_obj);
if(it != table.end())
{
DynamicAABBNode* node = it->second;
if(!node->bv.equal(updated_obj->getAABB()))
dtree.update(node, updated_obj->getAABB());
}
setup_ = false;
}
template <typename S>
void DynamicAABBTreeCollisionManag
8000
er<S>::update(const std::vector<CollisionObject<S>*>& updated_objs)
{
for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
update_(updated_objs[i]);
setup();
}TL;DRI'm not sure if we need to rebalance the dtree at some point. I would think that this is needed when adding a new collision object or after every N objects. We should use the When moving a collision object, I think that the unregister/register pattern should be used. When inserting a leaf node in the dtree, the bounding volumes are updated as necessary. This seems to corroborate the benchmark results. My bandwidth for this has exploded and cannot explore things more. I will add a better comment for why the register is needed. |
|
Actually there are 4 ways to update the broadphase manager (I updated the previous comments with a fourth option). The new update method is this one: So updating it this way is actually faster then the un/register method. So I will update my fix with this method instead.
|
|
Ok... so actually using Will surely revert to the original fix (reregister) because it is slightly faster then the |
Steps to update the AABB in the fcl broadphase manager: 1- Compute the collision geometry local AABB 2- Compute the collision object AABB 3- Update the AABB by reregistering the FCLObject
902e549 to
318645d
Compare
|
Found my bug, was wrongfully initialising the vector. Also I was probably not using enough samples. Bumped to 3000. Based on the benchmarks below, I don't think that a method is really superior then the other. So we probably should stick with the reregistering method for updating the AABB. This is what is actually used when creating an object. By using this method, the tree is not rebalanced, but it's not clear what is the effect based on the benchmarks. Furthermore, the I have re-pushed the initial fix with better comments including the git commit. Benchmark 1: Moving a primitive shape 3000 times by 0.25 metersThis test depends on the load on my PC, and was pretty inconsistent between runs. It's really fast so that maybe why: Q1 and Q3 around 370 nanoseconds. It benchmarks the Benchmark 2: Moving meshes 20@25k + 1@161kThe planning scene has an additional 96 meshes @25k that are not moved. It benchmarks the |
|
@simonschmeisser Can you retrigger the CI, it fails to fetch dependencies. |
…MOVE action PR moveit#3601 did not account for the cache (or other initialization) when moving an object. Branch to the `createCollisionGeometry` method where the cache is applied, but use the already populated collision geometry instead of recreating from scratch.
…MOVE action PR moveit#3601 did not account for the cache (or other initialization) when moving an object. Branch to the `createCollisionGeometry` method where the cache is applied, but use the already populated collision geometry instead of recreating from scratch.
Description
Related to #3599.
Updating a MoveIt Collision object shape poses takes 0.3 seconds for an object with 3 meshes with a total of 48,064 triangles.
When the FCL collision environment is notified by a World::CREATE | MOVE_SHAPE | ADD_SHAPE | REMOVE_SHAPE, the object is reconstructed from scratch as seen here.
This PR only updates the FCL object transforms when encountering the
MOVE_SHAPEaction. The time is reduced to an average of 16 microseconds.