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

testing

This commit is contained in:
cosmic_power 2024-09-27 00:03:07 +08:00
parent 91faa14682
commit 715e8f050a
3 changed files with 46 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Identification_main.m
View File

@ -13,7 +13,7 @@ robot = get_robot_R1000(file,opt);
robot = get_Kinematics(robot, opt); robot = get_Kinematics(robot, opt);
%TODO verify kinematics via robotics toolbox or other software result %TODO verify kinematics via robotics toolbox or other software result
R1000_Dynamics; R1000_Dynamics_num;
% opt.Isreal = false; % opt.Isreal = false;
% robot = get_velocity(robot, opt); % robot = get_velocity(robot, opt);
% robot = get_regressor(robot,opt); % robot = get_regressor(robot,opt);

33
R1000_Dynamics_num.m
View File

@ -12,9 +12,9 @@ link_mass = robot.m;
com_pos = robot.com; com_pos = robot.com;
link_inertia = robot.I; link_inertia = robot.I;
thetalist = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
% Get general mass matrix % Get general mass matrix
Glist=[]; Glist=[];
@ -29,10 +29,14 @@ for i = 0:N-1
if i == 0 if i == 0
M = robot.T(:,:,i+1)*transl(com_pos(:,i+1)); M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
else else
M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); M = TransInv(T(:,:,i+1))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
end end
Mlist_CG = cat(3, Mlist_CG, M); Mlist_CG = cat(3, Mlist_CG, M);
end 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]]; M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist_CG = cat(3, Mlist_CG, M); Mlist_CG = cat(3, Mlist_CG, M);
@ -62,9 +66,9 @@ J=sym(zeros(6,N));
exf=[0;0;0;0;0;0]; exf=[0;0;0;0;0;0];
for i = 1:length(q_J) for i = 1:length(q_J)
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ... [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ... = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
[0;0;-9.8], exf, Mlist_CG, Glist, Slist); [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)'); G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, 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 %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin
@ -74,13 +78,24 @@ end
% above 2020b % above 2020b
% F_Simpack = pagetranspose(F_Simpack); % F_Simpack = pagetranspose(F_Simpack);
% below 2020b % below 2020b
F_Simpack = permute(F_Simpack,[2 1 3]);
figure(1)
for i = 1:3 for i = 1:3
subplot(3,1,i); subplot(3,1,i);
hold on; hold on;
%added minus, so should be the same as simpack %added minus, so should be the same as simpack
plot(time,-reshape(F_Simpack(1,i,:),[1,length(F_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) % plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
end end

40
get_robot_R1000.m
View File

@ -9,8 +9,8 @@ switch opt.robot_def
case 'SDH' case 'SDH'
case 'MDH' case 'MDH'
robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0]; robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493]; robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157]; robot.d = [0,0,0,0,0,0.28,0.40,0,0];
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
robot.link_type = ['R','R','R','R','R','R','R','R','P']; robot.link_type = ['R','R','R','R','R','R','R','R','P'];
otherwise otherwise
@ -51,17 +51,17 @@ switch opt.robot_def
robot.accd(ndof)=q_sym(ndof); robot.accd(ndof)=q_sym(ndof);
end end
% Dynamics parameters % Dynamics parameters
link_mass = [17.42,7.7,2.42,5.16,2.22,1.78,2.32,2.92 0.1]; link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
%TODO in process, seems axis_of_rot useless %TODO in process, seems axis_of_rot useless
axis_of_rot(:,1) = [0;0;1]; axis_of_rot(:,1) = [0;0;1];
axis_of_rot(:,2) = [0;0;1]; axis_of_rot(:,2) = [0;-1;0];
axis_of_rot(:,3) = [0;0;1]; axis_of_rot(:,3) = [0;-1;0];
axis_of_rot(:,4) = [0;0;1]; axis_of_rot(:,4) = [0;-1;0];
axis_of_rot(:,5) = [0;0;1]; axis_of_rot(:,5) = [0;0;1];
axis_of_rot(:,6) = [0;0;1]; axis_of_rot(:,6) = [1;0;0];
axis_of_rot(:,7) = [0;0;1]; axis_of_rot(:,7) = [0;0;-1];
axis_of_rot(:,8) = [0;0;1]; axis_of_rot(:,8) = [0;-1;0];
axis_of_rot(:,9) = [0;0;1]; axis_of_rot(:,9) = [0;0;0];
% 画图 % 画图
% com_pos(:,1) = [0;0;0.122]; % com_pos(:,1) = [0;0;0.122];
% com_pos(:,2) = [0.373;0;0]; % com_pos(:,2) = [0.373;0;0];
@ -76,12 +76,12 @@ switch opt.robot_def
com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3; com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3;
com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3; com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3;
com_pos(:,4) = [5.3924501e+01 -5.1146307e+00 -5.8247754e+01]'*10^-3; com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3;
com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
com_pos(:,6) = [-6.0339586e-02 2.8867234e+01 4.9027126e+01]'*10^-3; com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
com_pos(:,7) = [ -2.3140597e-02 -9.5969566e+00 1.0229381e+02]'*10^-3; com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
com_pos(:,8) = [1.4847544e+01 6.8637348e+01 -1.5713395e-01]'*10^-3; com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
com_pos(:,9) = [0;0;0]; com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
% the inertia tensor wrt the frame oriented as the body frame and with the % the inertia tensor wrt the frame oriented as the body frame and with the
% origin in the COM % origin in the COM
% link_inertia(:,:,1) = diag([1,1,1]); % link_inertia(:,:,1) = diag([1,1,1]);
@ -114,10 +114,12 @@ switch opt.robot_def
link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];... link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];...
[-3.6340953e+00 1.3222824e+04 -4.3625500e+02];... [-3.6340953e+00 1.3222824e+04 -4.3625500e+02];...
[ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6; [ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6;
link_inertia(:,:,8) = [[4.6491328e+03 3.0225715e+03 2.8800116e+01];... link_inertia(:,:,8) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];...
[3.0225715e+03 8.8414058e+04 -3.0084286e+01];... [-4.6245133e+01 5.9901606e+04 6.0350548e+0];...
[2.8800116e+01 -3.0084286e+01 9.1858852e+04]]*10^-6; [4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6;
link_inertia(:,:,9) = diag([1,1,1]); link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];...
[3.3834506e+0 1.7996852e+03 5.2088778e+0];...
[4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6;
% verify if link_inertia is issymmetric % verify if link_inertia is issymmetric
for i = 1:ndof for i = 1:ndof
if(issymmetric(link_inertia(:,:,i))==false) if(issymmetric(link_inertia(:,:,i))==false)