function M = PlanMotions(s,g,N,R,vmax)
H = [];
M = CHOPstoMotionPlan(s,g,H,vmax);
[tc, Rc] = FindFirstCollision(M,R);
while(~isempty(tc))
    
end
end