108 lines
3.3 KiB
Matlab
108 lines
3.3 KiB
Matlab
%% R1000
|
|
N=9;
|
|
% traj
|
|
time = 0:0.01:1;
|
|
f=1;
|
|
q_J = sin(2*pi*f*time);
|
|
qd_J = (2*pi*f)*cos(2*pi*f*time);
|
|
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
|
|
zero_ = zeros(1,length(q_J));
|
|
% Dynamics parameters
|
|
link_mass = robot.m;
|
|
com_pos = robot.com;
|
|
link_inertia = robot.I;
|
|
|
|
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
|
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
|
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
|
|
|
% Get general mass matrix
|
|
Glist=[];
|
|
for i = 1:N
|
|
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
|
Glist = cat(3, Glist, Gb);
|
|
end
|
|
|
|
% Get the com pos transformation in each joint reference frame
|
|
Mlist_CG = [];
|
|
for i = 0:N-1
|
|
if i == 0
|
|
M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
|
else
|
|
M = TransInv(T(:,:,i+1))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
|
end
|
|
Mlist_CG = cat(3, Mlist_CG, M);
|
|
end
|
|
for i = i:N
|
|
M = transl(com_pos(:,i))
|
|
Mlist_CG = cat(3, Mlist_CG, M);
|
|
end
|
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
|
Mlist_CG = cat(3, Mlist_CG, M);
|
|
|
|
% Get the end efforce transformation in each joint reference frame
|
|
Mlist_ED = [];
|
|
for i = 1:N
|
|
M = robot.T(:,:,i);
|
|
Mlist_ED = cat(3, Mlist_ED, M);
|
|
end
|
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
|
Mlist_ED = cat(3, Mlist_ED, M);
|
|
|
|
%TODO: Get Slist form DH table method
|
|
% RRRRRRRRP
|
|
Slist=[[0;0;1;0;0;0],...
|
|
[0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
|
|
[0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
|
|
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
|
|
[0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
|
|
[1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
|
|
[0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
|
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
|
[0;0;0;1;0;0]];
|
|
|
|
Vlinear=sym(zeros(3,3));
|
|
J=sym(zeros(6,N));
|
|
exf=[0;0;0;0;0;0];
|
|
|
|
for i = 1:length(q_J)
|
|
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
|
|
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
|
|
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
|
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
|
|
T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
|
|
%Want to get the result from TC_delta, which means F at CG represent under frame at the last origin
|
|
F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
|
end
|
|
% plot Torque
|
|
% above 2020b
|
|
% F_Simpack = pagetranspose(F_Simpack);
|
|
% below 2020b
|
|
|
|
for i = 1:3
|
|
subplot(3,1,i);
|
|
hold on;
|
|
%added minus, so should be the same as simpack
|
|
plot(time,taumat(i+6,:))
|
|
xlabel('time(s)')
|
|
ylabel('Torque(Nm)')
|
|
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
|
end
|
|
|
|
F_Simpack = permute(F_Simpack,[2 1 3]);
|
|
figure(2)
|
|
for i = 1:3
|
|
subplot(3,1,i);
|
|
hold on;
|
|
%added minus, so should be the same as simpack
|
|
plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
|
|
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
|
end
|
|
|
|
% Use Body Twist cal linear vel, but can't cal the end frame vel
|
|
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
|
% [0;0;0], exf, Mlist, Glist, Slist);
|
|
% j=1;
|
|
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
|
|
% j=2;
|
|
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23); |