Added in checking for non-default sphere groups. Also added in low-res collision checking. Not finished yet.
Finished more of the hierarchical collision checking. Recorded videos using the new test file. I think I made the main function threadsafe but it's untested.
Collected more stats on collision checking. Documented them in Handoff Planner doc. Seems like multi-level check isn't faster. Mainly because FK is slow to compute twice. I'll restructure the code now, to compute all of the frames once per arm.
Added another overloaded computeFK with a small optimization. Only copies & reorders the angles vector once now.
Added param to KDL robot model for choosing hard/safety joint limits. Added function to CC to set attached objects.