planning stuff works, IK refinement in progress