Code covered by the BSD License

# Control optimization of a 4DOF arm using DIDO

### Alan Jennings (view profile)

30 Aug 2010 (Updated )

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

Arm_4DOF_DIDO.m
```% This program is adapted from the DIDO brachistochrone example

%===================
% Problem variables:
%-------------------
% states = (x, y, v)
% controls = theta
%===================
%---------------------------------------
% bounds the state and control variables
%---------------------------------------
% States: B, R, Theta, U then velocities (rad),
% as shown on the Motoman brochure for the IA20
% http://www.motoman.com/datasheets/discontinued/IA20.pdf
bounds.upper.states = pi/180*[120, 180, 125, 180,...
300, 300, 170, 170]';
bounds.lower.states = -bounds.upper.states;
% For a 90 to -90 movement performed in 2 sec, the control peaks at
% [10.2104   10.7841   61.0422   86.4997] Nm
% This is further refined from the ramp to constant velocity control
bounds.lower.controls = -[10, 4, 50, 7].';
bounds.upper.controls = -bounds.lower.controls;

%------------------
% bound the horizon
%------------------
t0 = 0;
tfMax = 2;
bounds.lower.time = [t0; tfMax];
bounds.upper.time = [t0; tfMax]; % Fixed final time problem
%-------------------------------------------
% Set up the bounds on the endpoint function
%-------------------------------------------
% See events file for definition of events function
% This is set up for the inital and final conditions
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];
bounds.lower.events = [Pose1.'; Pose2.'];
bounds.upper.events = bounds.lower.events;
% equality event function bounds
%============================================
% Define the problem using DIDO expresssions:
%============================================
Arm_4Dof.cost = 'Arm_4DOF_MinEnergy_Cost';
Arm_4Dof.dynamics = 'Arm_4DOF_Dynamics';
Arm_4Dof.events = 'Arm_4DOF_FixedFinal';
%Path file not required for this problem formulation;
% Path files are used to establish constraints during the motion
Arm_4Dof.bounds = bounds;
%====================================================
% algorithm.nodes = 36;
algorithm.nodes = 64;
% represents some measure of desired solution accuracy

% Compose initial guess to aid in computuation
[t_sim,State,Control_Out]=Arm_4DOF_PathCompose(Pose1,Pose2,tfMax);
% this program regulates the motion to a ramp to constant velocity motion.
algorithm.guess.states = State.';
algorithm.guess.controls = Control_Out.';
algorithm.guess.time = t_sim(:).';
algorithm.guess.nodes = t_sim(:).';
% nodes field is not necessary,
% but provides consistant notation for analysis

% % If you have a previous run, by all means, use it
% t_temp=linspace(primal.nodes(1),primal.nodes(end),151);
% algorithm.guess.states = interp1(primal.nodes,primal.states.',t_temp,'pchip').';
% algorithm.guess.controls = interp1(primal.nodes,primal.controls.',t_temp,'pchip').';
% algorithm.guess.time = t_temp(:).';
% algorithm.guess.nodes = t_temp(:).';
% clear t_temp primal

%% Call dido
tStart= cputime; % start CPU clock
% Make sure and initialize DIDO prior to calling dido
[cost, primal, dual] = dido(Arm_4Dof, algorithm);
runTime = cputime-tStart;
save(['Results_' date])
fprintf('Run time is: %g days, %g hours, %g min\n',...
floor(runTime/(60*60*24)), floor(mod(runTime/(60*60),24)), mod(runTime/(60),60));

figure;
plot(primal.nodes,primal.states,'-',primal.nodes,primal.controls,'o-')

```