closest point calculation, tested on arm_<*>_7_joint
sot compensation, no origin
only parsing capsule collision objects
dynamic self collision enabling
updated point return with full pose rather than identity rotation
cleanup