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

Arm_4DOF_ResultsCheck
load Results_09-Sep-2010.mat

% initial pose
fprintf('Showing Initial Pose (make sure model is set to animate)\n')
open('Arm_4DOF_1b.mdl')
sim('Arm_4DOF_1b.mdl',primal.nodes(1:2),...
     simset('InitialState',primal.states(:,1).'),...
    [primal.nodes(:),primal.controls.']);
% pause;
% final pose
fprintf('Showing Final Pose\n')
sim('Arm_4DOF_1b.mdl',primal.nodes(end-1:end),...
     simset('InitialState',primal.states(:,end).'),...
    [primal.nodes(:),primal.controls.']);
% pause;
fprintf('Animating Initial Response\n')
[t_guess,State_guess,Output_guess]=sim('Arm_4DOF_1b.mdl',...
    algorithm.guess.time,...
     simset('InitialState',algorithm.guess.states(:,1).'),...
    [algorithm.guess.time(:),algorithm.guess.controls.']);
% pause;
fprintf('Animating unrefined DIDO Response\n')
[t,State,Output]=sim('Arm_4DOF_1b.mdl',primal.nodes,...
     simset('InitialState',primal.states(:,1).'),...
    [primal.nodes(:),primal.controls.']);

Temp_Guess.nodes=algorithm.guess.time(:,1:10:end);
Temp_Guess.controls=algorithm.guess.controls(:,1:10:end);
Temp_Guess.states=algorithm.guess.states(:,1:10:end);

DynCheck=Arm_4DOF_Dynamics(primal);
Showing Initial Pose (make sure model is set to animate)
Showing Final Pose
Animating Initial Response
Animating unrefined DIDO Response
t_tune=linspace(primal.nodes(1),primal.nodes(end),201);
% pchip interpolation seems to do better than spline, especially regarding
% saturation
ControlGuess=interp1(primal.nodes,primal.controls.',t_tune,'pchip').';
Control_DIDO=interp1(primal.nodes,primal.controls.',t_tune,'pchip').';
PathGuess=interp1(primal.nodes,...
    primal.states([1,5, 2,6, 3,7, 4,8],:).',t_tune,'pchip').';

figure(410);clf;hold on;
plot(primal.nodes,primal.states.','o-')
plot(t_tune,PathGuess.','-');
title({'This plot shows the difference due to interpolation of the path'})
xlabel('Time (sec)')
ylabel('States (rad, rad/sec)')
temp=PathGuess([1,3,5,7,2,4,6,8],:);

DIDO_Deriv=DynCheck;
Prime=@(X,Y) [(-Y(3)+4*Y(2)-3*Y(1))./(-X(3)+4*X(2)-3*X(1)),...
    (Y(3:end)-Y(1:end-2))./(X(3:end)-X(1:end-2)),...
	(3*Y(end)-4*Y(end-1)+Y(end-2))./(3*X(end)-4*X(end-1)+X(end-2))];
tempDeriv_num=zeros(size(DIDO_Deriv,1),length(t_tune));
for amy=1:size(DIDO_Deriv,1)
    tempDeriv_num(amy,:)=Prime(t_tune,temp(amy,:));
end
clear temp amy

figure(411);clf;
subplot(211);hold on;
plot(primal.nodes.',DIDO_Deriv(1:4,:).','-s');
plot(t_tune,tempDeriv_num(1:4,:).','-');
plot(t_tune(1:20:end),tempDeriv_num(1:4,1:20:end).','+');
title({'Comparison of how well the spline (+) gives the same derivative as DIDO (s)'})
ylabel('Position derivative (rad/sec)');
subplot(212);hold on;
plot(primal.nodes.',DIDO_Deriv(5:8,:).','-s');
plot(t_tune,tempDeriv_num(5:8,:).','-');
plot(t_tune(1:20:end),tempDeriv_num(5:8,1:20:end).','+');
title({'Square, DIDO; Cross, Spline differentiation'})
ylabel('Velocity derivative (rad/sec^2)');
ControlGuess2=ControlGuess;
BestControlGuess2=ControlGuess2;
BestAccuracy2=inf;
BestItter2=1;
History2=zeros(1000,8);

[t2b,State2b]=sim('Arm_4DOF_1b.mdl',t_tune,...
     simset('InitialState',PathGuess([1,3,5,7, 2,4,6,8],1).'),...
    [t_tune(:),ControlGuess2.']);
figure(19)
subplot(211)
plot(t_tune,PathGuess(1:2:end,:).','r-',...
    t2b,State2b(:,1:4),'g-');
axis([0,2,-1.6,2.2]);
title(['Itteration: ', num2str(0)]);
subplot(212)
plot(t_tune,PathGuess(2:2:end,:).','r-',...
    t2b,State2b(:,5:end),'g-');
axis([0,2,-6,6]);

gain to adjust control based on path error

ReinforceGains=0.01*[...
    1,0,0,0, 2,0,0,0,;...
    0,1,0,0, 0,2,0,0,;...
    0,0,1,0, 0,0,2,0,;...
    0,0,0,1, 0,0,0,2].';
% Weighting function to help stabilize learning
Weight=@(x) 1.4*x./(x.^2+.5);

fprintf('\n Refining control for propogation of numeric error.');
fprintf('.. This may take a while\n\n');
for alice=1:size(History2,1)

[t2b,State2b]=sim('Arm_4DOF_1b.mdl',t_tune,...
     simset('InitialState',PathGuess([1,3,5,7, 2,4,6,8],1).'),...
    [t_tune(:),ControlGuess2.']);
ControlGuess2=ControlGuess2-[zeros(4,0),...
    (Weight(State2b(1:end-0,:)-PathGuess([1,3,5,7, 2,4,6,8],1:end-0).')*ReinforceGains).'];
% State2b: [All Angles; All Velocities]

Accuracy_all=State2b(end,:)-Pose2(:).';
Accuracy=norm(Accuracy_all);
History2(alice,:)=Accuracy_all;

if ~mod(alice,10)
% figure(18)
% subplot(311)
% plot(t2b,State2b(:,1:4)-PathGuess([1,3,5,7,],:).');
% axis([0,2,-5,5])
% subplot(312)
% plot(t2b,State2b(:,5:end)-PathGuess([2,4,6,8],:).');
% axis([0,2,-5,5])
% subplot(313)
% plot(t2b,(Weight(State2b-PathGuess([1,3,5,7, 2,4,6,8],:).')*ReinforceGains).');
figure(19)
subplot(211)
plot(t_tune,PathGuess(1:2:end,:).','r-',...
    t2b,State2b(:,1:4),'g-');
axis([0,2,-1.6,2.2]);
title(['Itteration: ', num2str(alice)]);
ylabel('Angles (rad)');
xlabel('Time (sec)');
subplot(212)
plot(t_tune,PathGuess(2:2:end,:).','r-',...
    t2b,State2b(:,5:end),'g-');
axis([0,2,-6,6]);
title(['Final Error Scalar: ', num2str(Accuracy), ', Best : ',...
     num2str(BestAccuracy2), ' at itteration ',  num2str(BestItter2)]);
ylabel('Angular velocity (rad/sec)');
xlabel('Time (sec)');
figure(13)
plot(1:alice,History2(1:alice,:),'+-',...
    BestItter2,History2(BestItter2,:),'ro');
title('Progression of refinement')
xlabel('Itteration')
ylabel('Final State Error')
end


if Accuracy<BestAccuracy2
BestControlGuess2=ControlGuess2;
BestItter2=alice;
BestAccuracy2=Accuracy;
end
if Accuracy<10^-3
    break
end
end
 Refining control for propogation of numeric error... This may take a while

ControlGuess2=BestControlGuess2;
[t2b,State2b,Output2b]=sim('Arm_4DOF_1b.mdl',t_tune(1:end),...
     simset('InitialState',PathGuess([1,3,5,7, 2,4,6,8],1).'),...
    [t_tune(:),ControlGuess2.']);
ControlGuess=ControlGuess2;
BestAccuracy=BestAccuracy2;
BestItter=BestItter2;
History=History2;
BestControlGuess=BestControlGuess2;

t2=t2b;
State2=State2b;
Output2=Output2b;
load('InitialGuess.mat','algorithm');

figure(221);clf;
subplot(211);hold on;
plot(primal.nodes([1,end]),[Pose1(1:4).',Pose2(1:4).'],'r^')
plot(primal.nodes,primal.states(1:4,:),'-')
plot(t_tune(5:20:end),PathGuess(1:2:end,5:20:end),'x')
plot(algorithm.guess.time,algorithm.guess.states(1:4,:),'-');
plot(algorithm.guess.time(10:20:end),algorithm.guess.states(1:4,10:20:end),'s');
title('DIDO Result (x) compared to Traditional path (s): Position');
axis tight
subplot(212);hold on
% plot(primal.nodes([1,end]),[Pose1(5:8).',Pose2(5:8).'],'r^')
plot(primal.nodes,primal.states(5:end,:),'-')
plot(t_tune(5:20:end),PathGuess(2:2:end,5:20:end),'x')
plot(algorithm.guess.time,algorithm.guess.states(5:end,:),'-');
plot(algorithm.guess.time(10:20:end),algorithm.guess.states(5:end,10:20:end),'s');
title('DIDO Result (x) compared to Traditional path (s): Velocity');
axis tight

figure(222);clf;hold on
plot(primal.nodes,primal.controls,'o-');
plot(t2,ControlGuess,'-');
plot(t2(5:8:end),ControlGuess(:,5:8:end),'x');
plot(algorithm.guess.time,algorithm.guess.controls,'-');
plot(algorithm.guess.time(10:20:end),algorithm.guess.controls(:,10:20:end),'s');
title(['DIDO Control Signals (o) compared to reinforcement learned ',...
    'control (x) and inital guess (s)']);
figure(321);clf;
subplot(331);hold on;
% Third column is for space for legend
plot(primal.nodes,primal.states(1:4,:),'-','linewidth',3)
% % Uncomment to verify good agreement
% plot(t_tune,PathGuess(1:2:end,:),'x')
plot(primal.nodes([1,end]),[Pose1(1:4).',Pose2(1:4).'],'r^')
title('Optimized: Position');
ylabel('(rad)')
axis([0,2,-1.6,2.4])
grid on
subplot(332);hold on;
plot(algorithm.guess.time,algorithm.guess.states(1:4,:),'-','linewidth',3);
plot(primal.nodes([1,end]),[Pose1(1:4).',Pose2(1:4).'],'r^')
title('Traditional: Position');
axis([0,2,-1.6,2.4])
ylabel('(rad)')
grid on
legend({'B','R','U','\theta'})

subplot(334);hold on
plot(primal.nodes,primal.states(5:end,:),'-','linewidth',3)
% % Uncomment to verify good agreement
% plot(t_tune,PathGuess(2:2:end,:),'x')
plot(primal.nodes([1,end]),[Pose1(5:end).',Pose2(5:end).'],'r^')
title('Optimized: Velocity');
ylabel('(rad/sec)')
axis([0,2,-6,6])
grid on
subplot(335);hold on
plot(algorithm.guess.time,algorithm.guess.states(5:end,:),'-','linewidth',3);
plot(primal.nodes([1,end]),[Pose1(5:end).',Pose2(5:end).'],'r^')
title('Traditional: Velocity');
ylabel('(rad/sec)')
axis([0,2,-6,6])
grid on

subplot(337);hold on
plot(primal.nodes,primal.controls,'-','linewidth',3);
% % Uncomment to verify good agreement
% plot(t2,ControlGuess,'x-');
title('Optimized: Control');
ylabel('Nm')
axis([0,2,-50,10])
grid on
xlabel('time (sec)')
subplot(338);hold on
plot(algorithm.guess.time,algorithm.guess.controls,'-','linewidth',3);
title('Traditional: Control');
ylabel('Nm')
axis([0,2,-50,10])
xlabel('time (sec)')
grid on
fprintf('Animating refined DIDO response and storing data\n')
[t5,State5,Output5]=sim('Arm_4DOF_BodyMotion_1b.mdl',t_tune,...
     simset('InitialState',primal.states(:,1).'),...
    [t_tune(:),ControlGuess2.']);
% [t5,State5,Output5]=sim('Arm_4DOF_BodyMotion_1b.mdl',primal.nodes,...
%      simset('InitialState',primal.states(:,1).'),...
%     [t_tune(:),ControlGuess2.']);
% [t5,State5,Output5]=sim('Arm_4DOF_BodyMotion_1b.mdl',t_tune,...
%      simset('InitialState',PathGuess([1,3,5,7, 2,4,6,8],1).'),...
%     [t_tune(:),ControlGuess2.']);
Animating refined DIDO response and storing data
PertyColors=hsv(4);
figure(433);clf;hold on;axis equal;
for gabby=floor(linspace(1,length(t5),20))
    figure(433);
for hiedi=0:3
fill3([Output5(gabby,13+9*hiedi),Output5(gabby,16+9*hiedi),Output5(gabby,19+9*hiedi)],...
    [Output5(gabby,14+9*hiedi),Output5(gabby,17+9*hiedi),Output5(gabby,20+9*hiedi)],...
    [Output5(gabby,15+9*hiedi),Output5(gabby,18+9*hiedi),Output5(gabby,21+9*hiedi)],...
    PertyColors(hiedi+1,:));
end
title(num2str(t5(gabby)));
sim('Arm_4DOF_BodyMotion_1b.mdl',t5(gabby)+[0,0.002],...
     simset('InitialState',PathGuess([1,3,5,7, 2,4,6,8],gabby).'),...
    [t_tune(:),ControlGuess2.']);
% pause;
end
Guess_Price=quad(@(t) (sum(interp1(algorithm.guess.time,...
    algorithm.guess.controls.',t,'pchip').^2,2)).^.5,0,2);
DIDO_Price=quad(@(t) (sum(interp1(primal.nodes,...
    primal.controls.',t,'pchip').^2,2)).^.5,0,2);
Reinforce_Price=quad(@(t) (sum(interp1(t2,...
    ControlGuess.',t,'pchip').^2,2)).^.5,0,2);
Price_Compare_Reinforce=(Reinforce_Price-DIDO_Price)/DIDO_Price;
Price_Compare_Guess=(Guess_Price-DIDO_Price)/DIDO_Price;
Price_Compare_Guess_2=(Guess_Price-DIDO_Price)/Guess_Price;

fprintf('\n\n DIDO Cost   : %g \n ',DIDO_Price);
fprintf('Refined Cost: %g, %g%% compared to DIDO \n ',...
    Reinforce_Price,Price_Compare_Reinforce*100);
fprintf('Initial Cost: %g, %g%% compared to DIDO \n ',...
    Guess_Price,Price_Compare_Guess*100);
fprintf('Wow, that means DIDO has a %g%% lower cost, Way to Go! \n ',...
    Price_Compare_Guess_2*100);

 DIDO Cost   : 19.5112 
 Refined Cost: 19.8442, 1.70651% compared to DIDO 
 Initial Cost: 45.7218, 134.336% compared to DIDO 
 Wow, that means DIDO has a 57.3262% lower cost, Way to Go! 
 

Contact us