search

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS
379 stars 102 forks source link

Add multi-threaded support to TrajOpt #333

Closed Levi-Armstrong closed 1 year ago

Levi-Armstrong commented 1 year ago

This replaces #302 and surprised that it was not having issues because the Models themselves needs to be threadsafe. This is why that PR required the omp critical around the addHinge, but does not solve the issue where other constraints are also adding variables and other items to the model which was not threadsafe.

@marip8 I came up with a way to separate running trajopt with and without multi-threading but not sure if there is a better way.

Levi-Armstrong commented 1 year ago

@marip8 Another option is make these methods virtual method of BasicTrustRegionSQP and then create BasicTrustRegionSQPOMP which overloads these methods. I think this approach is cleaner what do you think?

Levi-Armstrong commented 1 year ago

Alright I decided to leverage inheritance which I think is cleaner. Also added multiple unit tests and benchmark.

Levi-Armstrong commented 1 year ago

I ran the benchmarks and heaptrack to confirm the changes to not impact the default behavior. The multi threaded approach has a marginal improvement for the planning unit test and memory is doubled based on heaptrack. This is most likely due to each of the operation are not leveraging the same thread pool so the thread local caching is not as effective. In tesseract_planning I will implement a version which leverage the executor so it may perform better.

marip8 commented 1 year ago

The multi threaded approach has a marginal improvement for the planning unit test and memory is doubled based on heaptrack.

Interesting. How hard are these unit test problems (number of waypoints, constraints, etc.)? I would have thought there would be a significant time improvement

I haven't had a chance to look through this thoroughly yet, but it seems like a reasonable approach

Levi-Armstrong commented 1 year ago

The multi threaded approach has a marginal improvement for the planning unit test and memory is doubled based on heaptrack.

Interesting. How hard are these unit test problems (number of waypoints, constraints, etc.)? I would have thought there would be a significant time improvement

The planning unit test takes like 23 seconds but only has 6 constraints and 8 costs. I think this is a small problem, but the other issue is I believe each use of omp has its own thread pool so the caching is not as effective hindering performance.

Levi-Armstrong commented 1 year ago

I will test it on the puzzle piece example.

Levi-Armstrong commented 1 year ago

After CI finishes I will merge, but it would be good if you all have a test case to try and report back.

Levi-Armstrong commented 1 year ago

I tried the puzzle piece example but it also showed no difference. I see the threads ramp up but does not make a difference for these use cases.