Effacer les filtres
Effacer les filtres

3 DOF robot torque not converging Robotics Toolbox

1 vue (au cours des 30 derniers jours)
Mohsina Zafar
Mohsina Zafar le 18 Juil 2019
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]);
L(1).m = 1; %Link masses
L(2).m = 4;
L(3).m = 3;
L(1).r=[0 0 -0.015]; %Link COG
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;
L(1).qlim=[deg2rad(50) deg2rad(180)]; %Link limits
L(2).qlim=[deg2rad(30) deg2rad(180)];
L(3).qlim=[deg2rad(0) deg2rad(118)];
robot=SerialLink(L); %define robot
robot.links(1).Jm = 2.1184*10^-4; %Motor Inertias
robot.links(2).Jm = 2.1184*10^-4;
robot.links(3).Jm = 0.02;
qm=[0 0 0]; %Initializing variables
QD=[0 0 0];
T_o=[0;0;0];
T_d=[0;0;0]; %desired torque
T_l=[0;0;0];
Thm=[0;0;0];
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;
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
Attached are the Simulink models to run the code

Réponses (0)

Catégories

En savoir plus sur Robotics dans Help Center et File Exchange

Produits


Version

R2016a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by