60 lines
1.7 KiB
Matlab
60 lines
1.7 KiB
Matlab
%% R1000
|
|
N=length(robot.theta);
|
|
% Dynamics parameters
|
|
link_mass = robot.m;
|
|
com_pos = robot.com;
|
|
link_inertia = robot.I;
|
|
|
|
q_sym = sym('q%d',[N,1],'real');
|
|
qd_sym = zeros(N,1);%sym('qd%d',[N,1],'real');
|
|
q2d_sym = zeros(N,1);%sym('qdd%d',[N,1],'real');
|
|
thetalist = q_sym(1:N);
|
|
dthetalist = qd_sym(1:N);
|
|
ddthetalist = q2d_sym(1:N);
|
|
|
|
% 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
|
|
|
|
|
|
Mlist_CG = robot.kine.Mlist_CG;
|
|
Mlist_ED = robot.kine.Mlist_ED;
|
|
|
|
%TODO: Get Slist form DH table method
|
|
% RRRRRRRRP
|
|
Slist=robot.slist;
|
|
|
|
Vlinear=sym(zeros(3,3));
|
|
J=sym(zeros(6,N));
|
|
exf=[0;0;0;0;0;0];
|
|
|
|
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
|
|
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
|
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
|
% G = FKinSpaceExpand_Sym(Mlist_CG, Slist, thetalist);
|
|
% T=FKinSpaceExpand_Sym(Mlist_ED, Slist, thetalist);
|
|
% F_Simpack = getSimpackF_Sym(G,T,Mlist_ED,Fmat);
|
|
|
|
%Gen Files
|
|
matlabFunction(tau_mat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
|
|
'Vars',{q_sym});
|
|
standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName);
|
|
% traj
|
|
% time = 0:1: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);
|
|
% 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
|
|
% [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); |