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

add all

This commit is contained in:
cosmic_power 2024-09-27 08:36:52 +08:00
parent 91faa14682
commit 8b3af47926
2 changed files with 8 additions and 6 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);

12
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=[];
@ -62,7 +62,7 @@ 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.8], exf, Mlist_CG, Glist, Slist);
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)'); G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
@ -80,7 +80,9 @@ 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,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
xlabel('time(s)')
ylabel('Torque(NM)')
% 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