= [1 4 5];
    plot3(R{j}(1,:),R{j}(2,:),R{j}(3,:),RR(j,:),'LineWidth',2)
  end
  axis equal; axis([-2 2 -2 2 0 1.5]);
  xlabel 'x [m]';
  ylabel 'y [m]';
  grid on
  
  % draw controls:
  ut  = q(i,end-1);
  uw  = q(i,end);
  L = cost.fcn(cost,q(i,plant.dyno)',zeros(length(plant.dyno)));
  
  utM = 10;
  uwM = 50;
  
  oo = [4 -3.07 0]/6.4; o1 = [-0.5 2 2.0]; o2 = [-0.5 2 1.6]; o3 = [-0.5 2 1.2];
  o0 = 1.5*ut/utM;
  plot3([o1(1) o1(1)+o0*oo(1)],[o1(2) o1(2)+o0*oo(2)],[o1(3) o1(3)+o0*oo(3)],'b','LineWidth'