Tested CommandPose a little. Moves too fast, need to change saturation gains in .yaml. Also, the path it chooses doesn't always seem to be the best. Maybe we should use trajopt or arm_navigation