search

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS
373 stars 101 forks source link

Trajopt optimizations #390

Closed rjoomen closed 4 months ago

rjoomen commented 4 months ago

I noticed significant inefficiencies in creating/updating constraints and costs. With some profiling using perf and hotspot I solved a number of issues leading to a nice speedup for problems with large numbers of variables.

rjoomen commented 4 months ago

Properly reserve SparseMatrix

Before: image After: image

rjoomen commented 4 months ago

exprToEigen initialize from triplets

Before, lots of time spent in coeffRef(): image After (now SparseVector is the remaining issue): image

rjoomen commented 4 months ago

Create SparseVector from doublets

Before: image After (all below threshold): image

Levi-Armstrong commented 4 months ago

Awesome! This was on my todo list.