Parallelization of Distance Computation

[lukas-ramlab]

Hello

I want to speed up the collision computation, and in a nutshell I want to do some proximity check, and only if it is true I want to compute the precise distance.

Something like:

#pragma omp parallel for
  for (int index = 0; index < data.activeCollisionPairs.size(); index++) {
    if (!data.activeCollisionPairs[index])
      continue;

    if (isFarApart(model.collisionPairs[index])
      continue;
      
       geometry_data.distance_results[index] =
        pinocchio::computeDistance(geometry_model, geometry_data, index);
  }

Is this thread safe, or is there some limitations that I need to be aware of?

I also read in the docs that there is the computeDistances which does take care of the parallelization and could be coupled with custom distance_functors. Though I felt the above approach seems more intuitive for my example.
Or are there advantages to one or the other?

[jorisv]

Hello @lukas-ramlab,

You can give a look to the broadphase algorithm. The default implementation will use a BVH to check if a collision computation should be done.
There is already a parallelized version of the broadphase algorithm. This version use openmp, like in your example.
It compute collision and not distance, but maybe you can adapt it to your use case.

Calling pinocchio::computeDistance is thread safe if you don't call it for the same collision pair in two different thread (on first collision pair processing, some memory allocation can happen).

pinocchio::computeDistances doesn't use parallelization.

[lukas-ramlab]

Hello @jorisv

Thanks for the clarification, that helps a lot!

Does that mean that I have to do a warm-up first call to the pinocchio::computeDistance to avoid that multiple threads execute the first-time-memory-allocation in parallel?

The broadphase algorithm helps a lot, I can directly do that for collision avoidance. I see in that implementation though, that separate model and data objects are used. Is this required, or can the same model / data across the threads?

from https://github.com/stack-of-tasks/pinocchio/blob/eecaea8eaf846ebecc44077d99ee6a35a3101b9b/include/pinocchio/collision/parallel/broadphase.hpp#L82C1-L93C4

#pragma omp parallel for schedule(static)
      for (i = 0; i < batch_size; i++)
      {
        const int thread_id = omp_get_thread_num();
        const Model & model = models[(size_t)thread_id];
        Data & data = datas[(size_t)thread_id];
        BroadPhaseManager & manager = broadphase_managers[(size_t)thread_id];
        res_[i] =
          computeCollisions(model, data, manager, q.col(i), stopAtFirstCollisionInConfiguration);
      }
    }
  }

[jorisv]

Does that mean that I have to do a warm-up first call to the pinocchio::computeDistance to avoid that multiple threads execute the first-time-memory-allocation in parallel?

It's only mandatory if you evaluate the same collision pair in two different thread. I don't see the use case for that.
If you want to be sure the first iteration of distance detection run as fast as the next one, warm up is a good idea.

I see in that implementation though, that separate model and data objects are used. Is this required, or can the same model / data across the threads?

computeCollisions will write in data. So if you want to avoid a race condition, you should avoid using same data in different threads.
model is read-only, so I think it's safe to use the same in multiple thread. This discussion say it's copied for performance issue, but since it's read only, I'm not sure it's true.

[lukas-ramlab]

Yes, avoiding race condition is quite crucial. So I'll stick with separated data objects. (Though no evaluation of same collision pair needed).

I'll see if I have time to verify that if there is really a speed up with separate models. Especially since these can be relatively large.

Thanks for the clarification.