30 lines
949 B
Matlab
30 lines
949 B
Matlab
function GravityForce = getGravityForce(thetalist,robot,opt)
|
|
dthetalist = zeros([robot.ndof,1]);
|
|
ddthetalist = zeros([robot.ndof,1]);
|
|
exf = zeros([6,1]);
|
|
|
|
% Get info from robot
|
|
Mlist_CG = robot.kine.Mlist_CG;
|
|
Mlist_ED = robot.kine.Mlist_ED;
|
|
Slist = robot.slist;
|
|
|
|
% Get general mass matrix
|
|
Glist=zeros(6,6,robot.ndof);
|
|
link_inertia = zeros(3,3,robot.ndof);
|
|
for i = 1:robot.ndof
|
|
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
|
|
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),robot.m(i)*diag([1,1,1])];
|
|
Glist(:,:,i) = Gb;
|
|
end
|
|
|
|
[~,~,~,Fmat,~] ...
|
|
= InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
|
|
robot.gravity, exf, Mlist_CG, Glist, Slist);
|
|
G = FKinSpaceExpand(Mlist_CG, Slist, thetalist);
|
|
T = FKinSpaceExpand(Mlist_ED, Slist, thetalist);
|
|
F_Simpack = getSimpackF(G,T,Mlist_ED,Fmat);
|
|
% get J7
|
|
permutationMatrix = diag([1,1,-1,1,1,-1]);
|
|
GravityForce = permutationMatrix*F_Simpack(:,7);
|
|
end
|