29 lines
1.0 KiB
Mathematica
29 lines
1.0 KiB
Mathematica
|
function robot = get_Kinematics(robot, opt)
|
||
|
|
switch opt.KM_method
|
||
|
|
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 = 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
|