30 lines
717 B
Plaintext
30 lines
717 B
Plaintext
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;
|
|
|
|
% 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];
|
|
|
|
for i = 1:N
|
|
gravityForces(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
|
|
Jacoblist(:,:,i) = JacobianSpace(Slist(:,1:i),thetalist(1:i));
|
|
end
|
|
|
|
for i = N:-1:1
|
|
gravityTorques(i) = transpose(Jacoblist(:,:,i))*gravityForces(:,i);
|
|
end |