Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dynamics match with simpack

This commit is contained in:
cosmic_power 2024-09-23 02:45:58 +08:00
parent ca038a0569
commit 057c2af8ba
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
R1000_Dynamics_num.m
View File

@ -12,9 +12,9 @@ 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_]';
thetalist = [q_J;zero_;zero_;zero_;zero_;q_J;zero_;zero_;zero_]';
dthetalist = [qd_J;zero_;zero_;zero_;zero_;qd_J;zero_;zero_;zero_]';
ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;qdd_J;zero_;zero_;zero_]';
% Get general mass matrix
Glist=[];
@ -24,17 +24,17 @@ for i = 1:N
end
% Get the com pos transformation in each joint reference frame
Mlist_GC = [];
Mlist_CG = [];
for i = 0:N-1
if i == 0
M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
else
M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
end
Mlist_GC = cat(3, Mlist_GC, M);
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_GC = cat(3, Mlist_GC, M);
Mlist_CG = cat(3, Mlist_CG, M);
% Get the end efforce transformation in each joint reference frame
Mlist_ED = [];
@ -64,9 +64,10 @@ exf=[0;0;0;0;0;0];
for i = 1:length(q_J)
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ...
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
[0;0;-9.8], exf, Mlist_GC, Glist, Slist);
G(:,:,:,i) = FKinSpaceExpand(Mlist_GC, Slist, thetalist(i,:)');
[0;0;-9.8], 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
@ -75,6 +76,7 @@ figure(1)
for i = 1:3
subplot(3,1,i);
hold on;
%added minus, so should be the same as simpack
plot(time,-reshape(F_Simpack(1,i,:),[1,length(F_Simpack)]))
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
end

BIN
simpack_data/R1000-DVT-statics.sbr Normal file
View File

Binary file not shown.