Scribd
Upload
15 views

Link Matlab

The document describes the inverse kinematics solution for a 3 link robot arm. It presents the closed-form solution by setting the arm link lengths as constants and using trigonometric equations to solve for the two joint angles given a target end effector position. It also shows a numerical solution using optimization for comparison.

Uploaded by

Nguyen Trong Tan
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
15 views

Link Matlab

The document describes the inverse kinematics solution for a 3 link robot arm. It presents the closed-form solution by setting the arm link lengths as constants and using trigonometric equations to solve for the two joint angles given a target end effector position. It also shows a numerical solution using optimization for comparison.

Uploaded by

Nguyen Trong Tan
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 3

http://faculty.salina.k-state.edu/tim/robot_prog/Arm_robots/inverseKin.

html
https://www.daslhub.org/unlv/wiki/doku.php?id=2_link_kinematics

global a1 a2 a3;
a1 = 2; a2 = 1.5; a3 = 1;
% The above constants should not be changed. Otherwise, some of the numbers
% in the program will not be correct. Note: If the variables (a1, a2,
% and a3) are kept as variables, then the closed form equations become
% very messy. So we will consider this as a fixed dimension robot.

E = [2; 3.2]; % Target end effector position

%% Reachability NOTE: When developing an inverse kinematic solution,


% do reachability after working out the
% inverse kinematic equations.
%
% If the target position is out side the robot's reach, then no
% solution is available. From the geometry of the robot, the reach of the
% robot is the length of the first arm (+-) the distance from the start
% to end of the second arm.
% c = norm([a2; a3]);
% minReach = a1 - c;
% maxReach = a1 + c;
%
% Check if the robot can reach the desired position
n = norm(E);
if n < 0.2 || n > 3.8
disp('The desired end effector position is not reachable');
return;
end
fprintf('Desired position: %f %f\n', E(1), E(2));

%% Closed form solution


%
% Use the Symbolic Math Toolbox interactively to find equations
% for joint angles.
%
% syms q1 q2;
% T = trchain2('R(q1) Tx(a1) R(q2) Tx(a2) Ty(a3)', [q1 q2]);
% x = T(1,3);
% y = T(2,3);
% % % --- x and y have some sin and cos terms of the same angles
% % % try x^2 + y^2, which is E'*E (dot product with self)
% % %
% simplify(x^2 + y^2)
% % ans =
% % 6*cos(q2) - 4*sin(q2) + 29/4
% This is of the form a*cos(q2) + b*sin(q2) = c

% This is of the form a*cos(q2) + b*sin(q2) = c


q2 = get_angle(6, -4, (E'*E - 29/4));
q2 = min(q2); % elbow up

%%
% Now for q1. We have one unknown (q1) and two equations
% (x == E(1), and y == E(2)). However, both equations use q1 as
% sin(q1) and cos(q1) functions. So we can use both equations
% to find cos(q1) and sin(q1) as a system of equations.
% Then use an inverse tanget function to get q1.
% First get numeric values for q2 terms.
C2 = cos(q2);
S2 = sin(q2);

% Symbolic math for finding the equations


%
% syms q1 q2;
% T = trchain2('R(q1) Tx(a1) R(q2) Tx(a2) Ty(a3)', [q1 q2]);
% x = T(1,3);
% y = T(2,3);
% Find and rearrange x and y in terms of C2 and S2
% x = (2 + 3*C2/2 - S2)*cos(q1) - (C2 + 3*S2/2)*sin(q1) == E(1)
% y = (C2 + 1.5*S2)*cos(q1) + (2 + 1.5*C2 - S2)*sin(q1) == E(2)
A = [(2 + 3*C2/2 - S2) -(C2 + 3*S2/2);
(C2 + 1.5*S2) (2 + 1.5*C2 - S2)];
Q1 = A\E; % Q1 is now vector [cos(q1); sin(q1)]
q1 = atan2(Q1(2), Q1(1));

fwd = fwd_kin([q1 q2]);


fprintf('Closed form 2:\n\tjoint angles: %f %f\n', q1, q2);
fprintf('\tposition = %f %f\n', fwd(1), fwd(2));

%% Numerical solution -- for comparison and example only


% Now, use numerical methods to solve the inverse kinematics problem

fun = @(q) norm(fwd_kin(q) - E);


Q = fminsearch(fun, [pi/4 -pi/4]);
fprintf('Numeric:\n\tjoint angles: %f %f\n', Q(1), Q(2));
E1 = fwd_kin(Q);
fprintf('\tposition: %f %f\n', E1(1), E1(2));

%% Plot it
T = trot2(q1)*transl2(a1,0);
p1 = T(1:2,3);
T = trot2(q1)*transl2(a1,0)*trot2(q2)*transl2(a2,0);
p2 = T(1:2,3);
T = trot2(q1)*transl2(a1,0)*trot2(q2)*transl2(a2,a3);
p3 = T(1:2,3);
figure, hold on
plot([0 p1(1)], [0 p1(2)], 'LineWidth', 3);
plot([p1(1) p2(1) p3(1)], [p1(2) p2(2) p3(2)], 'LineWidth', 3);
grid on
daspect([1 1 1])
title('Plot of Robot Arm');
hold off

%% Helper private functions


% solution to a*cos(theta) + b*sin(theta) = c
function theta = get_angle(a, b, c)
theta = zeros(1,2);
d = sqrt(a^2 + b^2 - c^2);
e = atan2(a,b);
theta(1) = atan2(c,d) - e;
theta(2) = atan2(c,-d) - e;
theta = wrapToPi(theta);
end

% The forward kinematics as a private function to the script


function p = fwd_kin(q)
global a1 a2 a3;
T = trot2(q(1))*transl2(a1,0)*trot2(q(2))*transl2(a2, a3);
p = T(1:2,3);
end

You might also like