219 lines
8.8 KiB
Matlab
219 lines
8.8 KiB
Matlab
function robot = get_Kinematics(robot, opt)
|
|
if(opt.Isreal)
|
|
switch opt.KM_method
|
|
case 'SCREW'
|
|
% Get transformation for each joint
|
|
thetalist = robot.theta;
|
|
Slist = robot.slist;
|
|
% TODO:move to lib
|
|
for j = 1:length(thetalist)
|
|
T=robot.Home.M(:,:,j);
|
|
for i = j: -1: 1
|
|
T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
|
|
end
|
|
robot.TW(:,:,j) = T;
|
|
end
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
robot.T(:,:,i) = robot.TW(:,:,i);
|
|
else
|
|
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
|
|
end
|
|
end
|
|
robot.Fkine = robot.TW(:,:,end);
|
|
|
|
%stack result into kine structure
|
|
robot.kine.TW = robot.TW;
|
|
robot.kine.T = robot.T;
|
|
robot.kine.R = robot.kine.T(1:3,1:3,:);
|
|
robot.kine.t = robot.kine.T(1:3,4,:);
|
|
robot.kine.Fkine = robot.Fkine;
|
|
% get the CG at the world base frame
|
|
% FIXME: Fix this hack
|
|
ct=[];
|
|
Mlist_CG_Base=[];
|
|
com_pos_R1 = robot.com_pos_R1;
|
|
com_pos_R2 = robot.com_pos_R2;
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
ct(:,i) = com_pos_R1(:,i);
|
|
elseif i< 9
|
|
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
|
else
|
|
% HACK
|
|
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
|
end
|
|
robot.Home.com(:,i) = ct(:,i);
|
|
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
|
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
|
end
|
|
robot.kine.Mlist_CG_Base = Mlist_CG_Base;
|
|
% get the CG at the last GC frame
|
|
Mlist_CG=[];
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
|
else
|
|
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
|
end
|
|
end
|
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
|
Mlist_CG = cat(3, Mlist_CG, M);
|
|
robot.kine.Mlist_CG = Mlist_CG;
|
|
|
|
% Get the end efforce transformation in each joint reference frame
|
|
Mlist_ED = [];
|
|
for i = 1:length(thetalist)
|
|
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);
|
|
robot.kine.Mlist_ED = Mlist_ED;
|
|
case 'SDH'
|
|
theta = robot.theta;
|
|
alpha = robot.alpha;
|
|
a = robot.a;
|
|
d = robot.d;
|
|
robot.Fkine = eye(4,4);
|
|
ndof = length(theta); % special for MDH
|
|
% init transform matrix
|
|
robot.R = zeros([3,3,ndof]);
|
|
robot.t = zeros([3,1,ndof]);
|
|
robot.T = zeros([4,4,ndof]);
|
|
for i = 1:ndof
|
|
robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));...
|
|
sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));...
|
|
0 sin(alpha(i)) cos(alpha(i))];
|
|
robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(i)];
|
|
Transform = eye(4,4);
|
|
Transform(1:3,1:3) = robot.R(:,:,i);
|
|
Transform(1:3,4) = robot.t(:,:,i);
|
|
robot.T(:,:,i) = Transform;
|
|
robot.Fkine = robot.Fkine*robot.T(:,:,i);
|
|
end
|
|
case 'MDH'
|
|
theta = robot.theta;
|
|
alpha = robot.alpha;
|
|
a = robot.a;
|
|
d = robot.d;
|
|
robot.Fkine = eye(4,4);
|
|
ndof = length(theta); % special for MDH
|
|
% init transform matrix
|
|
robot.R = zeros([3,3,ndof]);
|
|
robot.t = zeros([3,1,ndof]);
|
|
robot.T = zeros([4,4,ndof]);
|
|
for i = 1:ndof
|
|
robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;...
|
|
sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));...
|
|
sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))];
|
|
robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))];
|
|
Transform = eye(4,4);
|
|
Transform(1:3,1:3) = robot.R(:,:,i);
|
|
Transform(1:3,4) = robot.t(:,:,i);
|
|
robot.T(:,:,i) = Transform;
|
|
robot.Fkine = robot.Fkine*robot.T(:,:,i);
|
|
end
|
|
otherwise
|
|
disp('Bad opt.KM_method!')
|
|
return;
|
|
end
|
|
else
|
|
switch opt.KM_method
|
|
case 'SCREW'
|
|
% Get transformation for each joint
|
|
thetalist = robot.theta;
|
|
Slist = robot.slist;
|
|
% TODO:move to lib
|
|
for j = 1:length(thetalist)
|
|
T=robot.Home.M(:,:,j);
|
|
for i = j: -1: 1
|
|
T = MatrixExp6_Sym(VecTose3(Slist(:, i) * thetalist(i))) * T;
|
|
end
|
|
robot.TW(:,:,j) = T;
|
|
end
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
robot.T(:,:,i) = robot.TW(:,:,i);
|
|
else
|
|
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
|
|
end
|
|
robot.kine.R(:,:,i) = robot.T(1:3,1:3,i);
|
|
robot.kine.t(:,:,i) = robot.T(1:3,4,i);
|
|
end
|
|
robot.Fkine = robot.TW(:,:,end);
|
|
|
|
%stack result into kine structure
|
|
robot.kine.TW = robot.TW;
|
|
robot.kine.T = robot.T;
|
|
robot.kine.Fkine = robot.Fkine;
|
|
% get the CG at the world base frame
|
|
% FIXME: Fix this hack
|
|
ct=[];
|
|
Mlist_CG_Base=[];
|
|
com_pos_R1 = robot.com_pos_R1;
|
|
com_pos_R2 = robot.com_pos_R2;
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
ct(:,i) = com_pos_R1(:,i);
|
|
elseif i< 9
|
|
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
|
else
|
|
% HACK
|
|
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
|
end
|
|
robot.Home.com(:,i) = ct(:,i);
|
|
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
|
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
|
end
|
|
% get the CG at the last GC frame
|
|
Mlist_CG=[];
|
|
for i = 1:length(thetalist)
|
|
if i == 1
|
|
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
|
else
|
|
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
|
end
|
|
end
|
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
|
Mlist_CG = cat(3, Mlist_CG, M);
|
|
% Get the end efforce transformation in each joint reference frame
|
|
Mlist_ED = [];
|
|
for i = 1:length(thetalist)
|
|
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);
|
|
% stack result
|
|
robot.kine.Mlist_CG_Base = Mlist_CG_Base;
|
|
robot.kine.Mlist_CG = Mlist_CG;
|
|
robot.kine.Mlist_ED = Mlist_ED;
|
|
case 'SDH'
|
|
case 'MDH'
|
|
theta = robot.theta;
|
|
alpha = robot.alpha;
|
|
a = robot.a;
|
|
d = robot.d;
|
|
robot.Fkine = eye(4,4);
|
|
ndof = length(theta); % special for MDH
|
|
% init transform matrix
|
|
robot.R = sym(zeros([3,3,ndof]));
|
|
robot.t = sym(zeros([3,1,ndof]));
|
|
robot.T = sym(zeros([4,4,ndof]));
|
|
for i = 1:ndof
|
|
robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;...
|
|
sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));...
|
|
sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))];
|
|
robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))];
|
|
Transform = sym(eye(4,4));
|
|
Transform(1:3,1:3) = robot.R(:,:,i);
|
|
Transform(1:3,4) = robot.t(:,:,i);
|
|
robot.T(:,:,i) = Transform;
|
|
robot.Fkine = robot.Fkine*robot.T(:,:,i);
|
|
end
|
|
otherwise
|
|
disp('Bad opt.KM_method!')
|
|
return;
|
|
end
|
|
end |