started adding coupling terms for obstacle avoidance