function plot_robots(P, Q)
% Plot robot initial and final pose
hold on;
[m, ~] = size(P);
qx = Q(:,1); qy = Q(:,2);
px = P(:,1); py = P(:,2); % Extract x- and y-values for convenience

% Plot with a different color for each robot assignment
c = [(1:m)./m*1 + 0;  (m:-1:1)./m*.5 + 0.5; (m:-1:1)./m*1 + 0]';
scatter(px, py, 20, c, 'filled');
% Plot lines
for k = 1:m
   plot([px(k) qx(k)], [py(k) qy(k)], '-', 'Color', c(k,:));
end
% Plot final shape
scatter(qx, qy, 20, 'black');
axis equal;
end