Motion planning and obstacle avoidance for manipulator

Summary
Motion planning and obstacle avoidance for manipulator