Add multi-threaded support to TrajOpt

[Levi-Armstrong]

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]

@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]

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

[Levi-Armstrong]

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]

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]

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]

I will test it on the puzzle piece example.

[Levi-Armstrong]

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]

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.