function M = CHOPstoMotionPlan(s,g,H,vmax)
for i = 1:length(g)
    T{i} = {[0;sqrt((s(i,1)-g(i,1))^2 + (s(i,2)-g(i,2))^2)/vmax]};
    X{i} = {[s(i,:);g(i,:)]};
    %M(i) = piecewise(0<=t<sqrt((s(i,1)-g(i,1))^2 + (s(i,2)-g(i,2))^2)/vmax, ...
        %s(i,:)+t/(sqrt((s(i,1)-g(i,1))^2 + (s(i,2)-g(i,2))^2)/vmax)*(g(i,:)-s(i,:)));
end
for j = 1:length(H)
   M = H{j,9};
   for i = 1:length(H{j,2})
       T{i} = [T{i}; M{i,1}(2:end)];
       X{i} = [X{i}; M{i,2}(2:end,:)];
   end
end
for i = 1:length(T)
    M{i,1} = T{i};
    M{i,2} = X{i};
end
end