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

seems can't generate symbol solution

This commit is contained in:
cosmic_power 2024-09-23 02:57:34 +08:00
parent 057c2af8ba
commit 6883fcc8fb
2 changed files with 41 additions and 38 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_num; R1000_Dynamics;
% 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);

77
R1000_Dynamics.m
View File

@ -1,18 +1,16 @@
%% R1000 %% R1000
N=9; N=9;
% traj
time = 0:0.01:2;
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 % Dynamics parameters
link_mass = robot.m; link_mass = robot.m;
com_pos = robot.com; com_pos = robot.com;
% link_inertia = robot.I; link_inertia = robot.I;
link_inertia(:,:,1) = diag([1,1,1]);
link_inertia(:,:,2) = diag([1,1,1]);
link_inertia(:,:,3) = diag([1,1,1]);
link_inertia(:,:,4) = diag([1,1,1]);
link_inertia(:,:,5) = diag([1,1,1]);
link_inertia(:,:,6) = diag([1,1,1]);
link_inertia(:,:,7) = diag([1,1,1]);
link_inertia(:,:,8) = diag([1,1,1]);
link_inertia(:,:,9) = diag([1,1,1]);
q_sym = sym('q%d',[N,1],'real'); q_sym = sym('q%d',[N,1],'real');
qd_sym = sym('qd%d',[N,1],'real'); qd_sym = sym('qd%d',[N,1],'real');
@ -29,15 +27,29 @@ for i = 1:N
end end
% Get the com pos transformation in each joint reference frame % Get the com pos transformation in each joint reference frame
Mlist = []; Mlist_CG = [];
for i = 1:N for i = 0:N-1
M = robot.T(:,:,i)+transl(com_pos(i)); if i == 0
Mlist = cat(3, Mlist, M); 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_CG = cat(3, Mlist_CG, M);
end 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 = cat(3, Mlist, M); 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 %TODO: Get Slist form DH table method
% RRRRRRRRP
Slist=[[0;0;1;0;0;0],... 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;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;0])]...
@ -46,7 +58,7 @@ Slist=[[0;0;1;0;0;0],...
[1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;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;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;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
[0;0;0;cross(-[0;0;0],[0.2+0.5+0.45+0.12+0.28-0.3157;0;-0.4-0.126493])]]; [0;0;0;1;0;0]];
Vlinear=sym(zeros(3,3)); Vlinear=sym(zeros(3,3));
J=sym(zeros(6,N)); J=sym(zeros(6,N));
@ -54,32 +66,23 @@ exf=[0;0;0;0;0;0];
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ... [V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ... = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
[0;0;-9.8], exf, Mlist, Glist, Slist); [0;0;-9.8], exf, Mlist_CG, Glist, Slist);
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist); % G = FKinSpaceExpand_Sym(Mlist_CG, Slist, thetalist);
% T=FKinSpaceExpand_Sym(Mlist_ED, Slist, thetalist);
% Get the end efforce transformation in each joint reference frame % F_Simpack = getSimpackF_Sym(G,T,Mlist_ED,Fmat);
Mlist = [];
for i = 1:N+1
M = robot.T(:,:,i);
Mlist = cat(3, Mlist, M);
end
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, Mlist, M);
T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
%Gen Files %Gen Files
matlabFunction(F_Simpack,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),... matlabFunction(Fmat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
'Vars',{q_sym,qd_sym,q2d_sym}); 'Vars',{q_sym,qd_sym,q2d_sym});
standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName); standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName);
% traj % traj
time = 0:1:2; % time = 0:1:2;
f=1; % f=1;
q_J = sin(2*pi*f*time); % q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(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); % qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J'); % F_Simpack_list = feval(standard_dynamics_func, q_J',qd_J',qdd_J');
% Use Body Twist cal linear vel, but can't cal the end frame vel % Use Body Twist cal linear vel, but can't cal the end frame vel
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ... % [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...