42 lines
994 B
Matlab
42 lines
994 B
Matlab
%% R1000
|
|
N=9;
|
|
time = 0:0.01:1;
|
|
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));
|
|
|
|
|
|
% thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
|
thetalist = zeros(N,1);
|
|
|
|
Mlist_CG = robot.kine.Mlist_CG;
|
|
Slist=robot.slist;
|
|
|
|
% Dynamics parameters
|
|
link_mass = robot.m;
|
|
com_pos = robot.com;
|
|
link_inertia = robot.I;
|
|
% 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
|
|
|
|
gravity = [0;0;-9.806];
|
|
gravityForcelist = zeros(6,N);
|
|
for i = 1:N
|
|
gravityForcelist(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
|
|
end
|
|
|
|
% JacobianMatrix = zeros(6*N,6*N);
|
|
% for i = 1:N
|
|
% for j = 1:N
|
|
% JacobianMatrix(1+6*(i-1):6*i,1+6*(j-1):6*j) = JacobianSpace(Slist(:,i:j),thetalist(i:j));
|
|
% end
|
|
% end
|
|
JacobianMatrix = JacobianSpace(Slist,thetalist);
|
|
gravityTorques = transpose(JacobianMatrix)*gravityForcelist;
|