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

a=Arm_4DOF_Dynamics(x)
```function a=Arm_4DOF_Dynamics(x)
% States: Tip to base(B, R, U, Theta), angle then velocity in radians
% use mech_import to get the model built
%
%% for testing individual states
% tracy=[pi/2*0.75,pi/2*1.5,pi/2*0.25,pi/2*0.5,...
%     0,0, 0,0];
% tracy=[pi/2*0,pi/2*0,pi/2*0,pi/2*0,...
%     0,0, 0,1000];
% Pose1=[pi/2*0.9,pi/2*-0.2,pi/2*0.8,pi/2*0.3,...
%     0,0, 0,0];
% Pose2=[pi/2*-0.1,pi/2*-0.2,pi/2*0.6,pi/2*1.2,...
%     0,0, 0,0];
% 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];
% tracy=Pose2;
% [t,State,Output]=sim('Arm_4DOF_1b.mdl',0+[0;0.001;0.002],...
%      simset('InitialState',tracy),...
%     [0+[0;0.02],0+[0;0], 0+[0;0], 0+[0;0],0+[0;0]])
%%
% % For testing a given series:
% x.nodes=0:4;
% x.states=[Pose1.',(3*Pose1+Pose2).'/4,(Pose1+Pose2).'/2,...
%     (Pose1+3*Pose2).'/4,Pose2.'];
% x.controls=zeros(4,5);

a=zeros(size(x.states));
for jerry =1:length(x.nodes)
[t,State,Output]=sim('Arm_4DOF_1b.mdl',x.nodes(jerry)+[0;0.001;0.002],...
simset('InitialState',x.states(:,jerry).'),...
[x.nodes(jerry)+[0;0.02],repmat(x.controls(:,jerry).',2,1)]);
a(:,jerry) = Output(1,[2,5,8,11, 3,6,9,12]).';
end

```