Added visualization of target while setting new goals. Todo: inverse kinematics