image thumbnail

Control optimization of a 4DOF arm using DIDO

by

 

30 Aug 2010 (Updated )

4 DOF arm imported via SimMechanics generates dynamics for optimal pick-place control solved by DIDO

[t3,State3,Control_Out]=Arm_4DOF_PathCompose(Pose1,Pose2,tf,PlotFigs)
function [t3,State3,Control_Out]=Arm_4DOF_PathCompose(Pose1,Pose2,tf,PlotFigs)
if nargin<=3
    PlotFigs=false;
end

tempE=cputime;
MaxSpeeds=[30,30,17,50]*pi/180;
Ao=MaxSpeeds/.35; %acceleration for ramp up

% test poses 
% Pose1=[pi/2*0.9,pi/2*-0.2,pi/2*0.8,pi/2*0,0,0, 0,0];
% Pose2=[pi/2*0.5,pi/2*0.2,pi/2*0.3,pi/2*0.8,0,0, 0,0];
% States are B, R, U, Theta; angles than velocity both in radians
% tf=2;
PulseTime=linspace(0,tf,401).';
dt=mean(diff(PulseTime));
tr=tf/2-(tf^2/4-abs(Pose2(1:4)-Pose1(1:4))./Ao).^.5;
% tr is rise time for each joint
PulsePath=zeros(length(PulseTime),4);
PulseVel=zeros(length(PulseTime),4);
PulseAcc=zeros(length(PulseTime),4);
Inputs=zeros(length(PulseTime),1+3*4); % see model for input order
Inputs(:,1)=PulseTime;
BestItter=1;
for alice=1:length(Ao); %do each state individually
    RampUp=PulseTime<=tr(alice);
    RampDown=PulseTime>=(tf-tr(alice));
PulseAcc(RampUp,alice)=sign(Pose2(alice)-Pose1(alice)).*Ao(alice);
PulseAcc(RampDown,alice)=-sign(Pose2(alice)-Pose1(alice)).*Ao(alice);
PulseVel(:,alice)=Ao(alice).*tr(alice).*sign(Pose2(alice)-Pose1(alice));
if tr(alice)>0
PulseVel(RampUp,alice)=Ao(alice).*tr(alice).*(PulseTime(RampUp)./tr(alice)).*...
    sign(Pose2(alice)-Pose1(alice));
PulseVel(RampDown,alice)=Ao(alice).*tr(alice).*((tf-PulseTime(RampDown))./tr(alice)).*...
    sign(Pose2(alice)-Pose1(alice));
end
PulsePath(:,alice)=Pose1(alice)+cumsum(PulseVel(:,alice))*dt;
% there is a better (more accurate) way of doing this
    Inputs(:,1+3*(alice-1)+(1:3))=[PulsePath(:,alice),PulseVel(:,alice),...
        PulseAcc(:,alice)];
end
[t,State,Output]=sim('Arm_4DOF_PathFollow_1b.mdl',PulseTime,...
     simset(),Inputs); 
d=Output(:,[1,5,9,13, 2,6,10,14]);
ControlGuess=Output(:,[4,8,12,16]);
History=zeros(91,8);
BestControlGuess=ControlGuess;
BestAccuracy=inf;
for alice=1:size(History,1)
[t2,State2,Output2]=sim('Arm_4DOF_Feedback_1b.mdl',PulseTime,...
     simset('InitialState',Pose1(:).'),...
    [PulseTime(:),ControlGuess, Inputs(:,[2,3, 5,6, 8,9, 11,12])]);
    ControlGuess=ControlGuess+Output2(:,9:12);
    % this model constrains motion and reports the nessary torques
Accuracy_all=Output2(end,[1,3,5,7, 2,4,6,8])-Pose2(:).';
Accuracy=norm(Accuracy_all);    
History(alice,:)=Accuracy_all;
    if PlotFigs
figure(12);clf;
subplot(311);hold on; 
plot(t,d(:,1:4),'-');plot(t(1:40:end),d(1:40:end,1:4),'o')
plot(t,State2(:,1:4),'-');plot(t(1:40:end),State2(1:40:end,1:4),'+')
axis([-0.01,2.01,-.5,1.5]);
title(['Itteration: ', num2str(alice)]);
subplot(312);hold on;
plot(t,d(:,5:8),'-');plot(t(1:40:end),d(1:40:end,5:8),'o')
plot(t,State2(:,5:8),'-');plot(t(1:40:end),State2(1:40:end,5:8),'+')
axis([-0.01,2.01,-1.,1.]);legend({'A','B','C','D'},'Location','SW');
title(['Final Error: ', num2str(Accuracy_all), ', Best error: ',...
    num2str(BestAccuracy), ' at itteration ' num2str(BestItter)]);
subplot(313);
plot(t,ControlGuess,'-');
title(['Final Error Scalar: [', num2str(Accuracy), ']']);

figure(13)
plot(1:alice,History(1:alice,:),'+-',BestItter,History(BestItter,:),'ro');
    end

if Accuracy<BestAccuracy
BestControlGuess=ControlGuess;
BestItter=alice;
BestAccuracy=Accuracy;
end
if Accuracy<10^-3
    break
end
end
Control_Out=BestControlGuess;

[t3,State3,Output3]=sim('Arm_4DOF_1b.mdl',PulseTime,...
     simset('InitialState',Pose1(:).'),...
    [PulseTime(:),BestControlGuess]);
Accuracy_all=Output3(end,[1,4,7,10,2,5,8,11])-Pose2(:).';
Accuracy=norm(Accuracy_all);    

if PlotFigs
figure(112);clf;
subplot(311);hold on; 
plot(t,d(:,1:4),'-');plot(t(1:40:end),d(1:40:end,1:4),'o')
plot(t,State3(:,1:4),'-');plot(t(1:40:end),State3(1:40:end,1:4),'+')
axis([-0.01,2.01,-.5,1.5]);
title(['Final Path, itteration: ', num2str(BestItter), ...
    ' Total Time:', num2str(cputime-tempE)]);
subplot(312);hold on;
plot(t,d(:,5:8),'-');plot(t(1:40:end),d(1:40:end,5:8),'o')
plot(t,State3(:,5:8),'-');plot(t(1:40:end),State3(1:40:end,5:8),'+')
axis([-0.01,2.01,-1.,1.]);legend({'A','B','C','D'},'Location','SW');
title(['Final Error: [', num2str(Accuracy_all), ']']);
subplot(313);
plot(t,ControlGuess,'-');
title(['Final Error Scalar: [', num2str(Accuracy), ']']);
figure(113)
plot(1:alice,History(1:alice,:),'+-',BestItter,History(BestItter,:),'ro');
end

Contact us