c_viewspli.m

多智能体工具包

% C_VIEWSPLI	Draw the spline trajectory of round box pushing.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

close all;
clear all;

%FLname='d:\maxplus2\circ\result\circ01';
FLname='d:\maxplus2\circ\result\circ02';

eval(['load ' FLname ';']);

figHistory=figure(...
            'Color',[0 0 0],...
            'Position',[100 0 600 600],...
            'Resize','on',...
            'NumberTitle','off',...
            'Name','Distribution Show');

  Point_X=0;
  Point_Y=0;
  View_Width=100;

  End_X=Point_X+View_Width;
  End_Y=Point_Y+View_Width;

BeginPoint=68;
EndPoint=131;

ROHistory=[RobotHistory(BeginPoint:EndPoint,:) ObjectHistory(BeginPoint:EndPoint,:)];
OHistory=ObjectHistory(BeginPoint:EndPoint,:);
ChHistory=ChromHistory(BeginPoint:EndPoint,:);
GHistory=GoalHistory(BeginPoint:EndPoint,:);

clf;                     

plot(Point_X,Point_Y,'color',[1 1 1]);
axis([Point_X,End_X,Point_Y,End_Y]);
axis on;                 
hold on;                 

SplineLength=EndPoint-BeginPoint+1;;

  for mm=1:4
    LocationX=ROHistory(SplineLength,(mm-1)*2+1);
    LocationY=ROHistory(SplineLength,mm*2);

    X_Vector=[];
    Y_Vector=[];
    for nn=1:SplineLength
      Last_X=ROHistory(nn,(mm-1)*2+1);
      Last_Y=ROHistory(nn,mm*2);
      X_Vector=[X_Vector Last_X];
      Y_Vector=[Y_Vector Last_Y];
    end    
    t=1:SplineLength;
    ts=1:0.1:SplineLength;
    xs=spline(t,X_Vector,ts);
    ys=spline(t,Y_Vector,ts);
    if mm==1
      plot(xs,ys,'b:');
    elseif mm==2
      plot(xs,ys,'r--'); 
    elseif mm==3
      plot(xs,ys,'g-.');
    elseif mm==4
      plot(xs,ys,'k-');
    end

    if mm==4
      plot(LocationX,LocationY,'Marker','o','Color','k','markersize',14);
    else
      ObjectPosition=OHistory(SplineLength,:);
      CurrentRobot=ROHistory(SplineLength,(mm-1)*2+1:2*mm);
      [FirstArray,SecondArray,Fill1Array,Fill2Array]=plotrob(ObjectPosition,CurrentRobot,RobotSize,ObjectSize);
      line(FirstArray,SecondArray);
      fill(Fill1Array,Fill2Array,'k');
    end;

  end
plot(70,90, 'Marker','^');
plot(25,57, 'Marker','d');
plot(GHistory(SplineLength,1), GHistory(SplineLength,2), 'Marker','+');
drawnow;
title('Motion Trajectory');


gtext('t=131');
gtext('t=80');
gtext('t=68');