Added trajectory planning, removed unnecessary package line, IK z axis still broken