tesseract-robotics / trajopt

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

Fix trajopt_sco exprToEigen large heap allocations #310

Closed Levi-Armstrong closed 1 year ago

Levi-Armstrong commented 1 year ago

This is to address issue #309. It turns out that it was allocating enough space to fill out the sparse matrix as if it was a dense matrix which really defeats the point of using a sparse matrix. This updated so item memory intensive variable is thread local and do not reserve space.

Levi-Armstrong commented 1 year ago

This results in an 15% reduction for what I would consider a large planning problem and 90% reductions in planning time for simple motion planning problem.

Before:

---------------------------------------------------------------------------------------------------
Benchmark                                                         Time             CPU   Iterations
---------------------------------------------------------------------------------------------------
BM_TRAJOPT_SIMPLE_COLLISION_SOLVE/real_time                    3204 us         3204 us          217
BM_TRAJOPT_PLANNING_SOLVE/min_time:6.000/real_time            46973 ms        46970 ms            1

After:

---------------------------------------------------------------------------------------------
Benchmark                                                   Time             CPU   Iterations
---------------------------------------------------------------------------------------------
BM_TRAJOPT_SIMPLE_COLLISION_SOLVE/real_time               355 us          355 us         1966
BM_TRAJOPT_PLANNING_SOLVE/min_time:6.000/real_time      39786 ms        39785 ms            1