werner291
commented
10 months ago Here's a MWE:
#include <chrono>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/geometry/shape/box.h>
#include <fcl/narrowphase/collision-inl.h>
#include <fcl/narrowphase/collision_object.h>
#include <fcl/narrowphase/collision_request.h>
#include <random_numbers/random_numbers.h>
int main(int argc, char** argv)
{
std::shared_ptr<fcl::BVHModel<fcl::OBBd>> without_RSS = std::make_shared<fcl::BVHModel<fcl::OBBd>>();
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> with_RSS = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();
random_numbers::RandomNumberGenerator rng(42);
const size_t N_TRIANGLES = 30000;
const double radius = 5.0;
without_RSS->beginModel();
with_RSS->beginModel();
for (size_t i = 0; i < N_TRIANGLES; ++i)
{
fcl::Vector3d a(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d b(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
fcl::Vector3d c(rng.uniformReal(radius, radius), rng.uniformReal(radius, radius), rng.uniformReal(radius, radius));
without_RSS->addTriangle(a, b, c);
with_RSS->addTriangle(a, b, c);
}
with_RSS->endModel();
without_RSS->endModel();
fcl::CollisionObjectd co_without(without_RSS, fcl::Transform3d::Identity());
fcl::CollisionObjectd co_with(with_RSS, fcl::Transform3d::Identity());
const auto box = std::make_shared<fcl::Boxd>(1.0, 0.1, 1.0);
size_t N_SAMPLES = 1000;
long time_without = 0;
long time_with = 0;
for (size_t sample_i = 0; sample_i < N_SAMPLES; ++sample_i)
{
// Generate a random transform.
fcl::Transform3d tf = fcl::Translation3d(rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius), rng.uniformReal(-radius, radius)) *
fcl::Quaterniond(rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0), rng.uniformReal(-1.0, 1.0)).normalized();
fcl::CollisionObjectd co(box, tf);
bool c1 = false;
bool c2 = false;
auto time_1 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_without, &co, req, res);
c1 = res.isCollision();
}
auto time_2 = std::chrono::high_resolution_clock::now();
{
fcl::CollisionRequestd req;
fcl::CollisionResultd res;
fcl::collide(&co_with, &co, req, res);
c2 = res.isCollision();
}
auto time_3 = std::chrono::high_resolution_clock::now();
time_without += std::chrono::duration_cast<std::chrono::nanoseconds>(time_2 - time_1).count();
time_with += std::chrono::duration_cast<std::chrono::nanoseconds>(time_3 - time_2).count();
assert(c1 == c2);
}
std::cout << "Time without RSS: " << time_without << std::endl;
std::cout << "Time with RSS: " << time_with << std::endl;
std::cout << "Ratio: " << (double)time_without / (double)time_with << std::endl;
}Output on my system (with -g -O3) reads:
Time without RSS: 141253
Time with RSS: 1136158
Ratio: 0.124325That's almost a 90% improvement (with a bit of an artificial example, admittedly, but still...)
I'll see if I can get some more stats.
github-actions[bot]
sjahr
Hello,
in my PhD project, I am frequently collision-checking against fairly large (>38k triangles) triangle meshes (it's a model of an apple tree; my planning space is highly cluttered).
In Moveit's FCL collision-checking environment, meshes are typically represented as a
fcl::BVHModel<fcl::OBBRSSd>, whereOBBRSSdis essentially a combination of an oriented bounding box and a rectangle sphere-swept bounding volume.Since I was only collision-checking rather than measuring distances, I tried using
`fcl::BVHModel<fcl::OBBd>instead, thus only using an OBB without the distance-computation acceleration data.To my surprise, the collision check ran over 60-70% faster in my particular circumstances. (Note that most collision checks failed.)
Since collision-checking is often the main performance bottleneck in motion planning, would there be any interest in me investigating this further and preparing a minimum working example? Is it common to have such large triangle meshes representing obstacles?
I'll almost certainly be working with this more, but I'd like to know if there's interest in integrating this into Moveit proper.