0
$\begingroup$

I have a Simulink model in which desired torque is [0;0;0] and current torque should converge to zeros too. But I am getting oscillatory torque as output.

This is the block diagram of what I am trying to implement: https://imgur.com/a/XhRgH91

This is the code I am implementing based to the block diagram:

l2=0.28;    %link length
l3=0.2;     %link length
d1=0.05;    %link offset

%D-H Parameters
L(1)= Link([0 0.03 0 -pi/2]);
L(2)= Link([0 0 l2 0]);
L(3)= Link([0 0 l3 0]);

%Link masses
L(1).m = 1; L(2).m = 4; L(3).m = 3; 
%Link COG
L(1).r=[0 0 -0.015]; L(2).r=[0.14 0 0]; L(3).r=[0.1 0 0];
%Link Inertias
ax1=0.03; ay1=0.03; az1=0.03;
ax2=0.28; ay2=0.05; az2=0.05;
ax3=0.2; ay3=0.05; az3=0.05;
I1=1/12*[ay1^2+az1^2 0 0; 0 ax1^2+az1^2 0; 0 0 ax1^2+ay1^2];
I2=4/12*[ay2^2+az2^2 0 0; 0 ax2^2+az2^2 0; 0 0 ax2^2+ay2^2];
I3=3/12*[ay3^2+az3^2 0 0; 0 ax3^2+az3^2 0; 0 0 ax3^2+ay3^2];   
L(1).I=I1; L(2).I=I2; L(3).I=I3;

%Link limits
L(1).qlim=[deg2rad(50) deg2rad(180)];
L(2).qlim=[deg2rad(30) deg2rad(180)];
L(3).qlim=[deg2rad(0) deg2rad(118)];

robot=SerialLink(L);    %define robot

%Motor Inertias
robot.links(1).Jm = 2.1184*10^-4;
robot.links(2).Jm = 2.1184*10^-4;
robot.links(3).Jm = 0.02;

%Initializing variables
qm=[0 0 0]; QD=[0 0 0]; T_o=[0;0;0]; T_l=[0;0;0]; Thm=[0;0;0];

T_d=[0;0;0];    %desired torque

load('Motor_Param_NEW.mat')     %Motor Parameters

tt=0:0.1:10;    %Time
for i = 1:length(tt)
    t = tt(i)
    T_e=T_d-Thm;
    T_e=[t*10 T_e'];    %converting to timeseries
    a1 = sim('Exo_control','SimulationMode','normal');
    out1 = a1.get('T_l');
    T_l = out1(11,:)';
    T_s=T_l+Thm;
%     plot(T_s,t)
%     hold on
    T_s=T_s';
    QDD = robot.accel(qm, QD, T_s);  %robot dynamics
    T_s=T_s';
    QDD=[t*10 QDD'];    %converting to time series
    a2 = sim('exo_integral','SimulationMode','normal');
    out2 = a2.get('QD');
    out3 = a2.get('Q')
    QD=out2(11,:);
    qm=out3(11,:);         %current joint angles

    pm = robot.fkine(qm);   %robot kinematics
    pm = [pm.t(1);pm.t(2);pm.t(3)]; %current end-effector position

    qh = [1 2 1.5]; %desired joint angles 
    ph = robot.fkine(qh);
    ph = [ph.t(1);ph.t(2);ph.t(3)]; %desired end-effector position

    pos = pm-ph;  %different between current & desired

    K=[1000 0 0;0 1000 0; 0 0 1000];    %gain
    Fhm = K*pos;                        %force

    J = robot.jacobe(qm);    %Jacobian
    J = J(1:3,:);           %linear velocity part of Jacobian
    Thm = J'*Fhm;           %Torque
end

Simulink models used are: https://drive.google.com/open?id=1oVZ-ttqy-_GYh7_bPNp0Ugg6NG39V0IU

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.