Optimal reachability with obstacle avoidance for hyper-redundant and soft manipulators