3 DOF robot torque not converging Robotics Toolbox

4 次查看（过去 30 天）
Mohsina Zafar
Mohsina Zafar 2019-7-18
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

请先登录，再回答此问题。

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息

标签

产品


版本

R2016a

Community Treasure Hunt

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

Start Hunting!

Translated by