modified ik_goal.py