Obstacle and self-collision avoidance for a dual arm robot