Modified robot final instruct - object avoidance
some changes