goToPose -> goToPoseSync+waitForMotionDone and setPosePriority. Working far better.
Merge branch 'develop' of https://github.com/praveenv4k/stochastic_planning into develop

Conflicts:
	config/master.json
	src/agent_controller/objectcontroller.cpp