48 lines
1.1 KiB
Matlab
48 lines
1.1 KiB
Matlab
function theta = inverseKinematics(x, y, L1, L2)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% Calculate inverse kinematics for a two-link planar robot
|
|
% Inputs:
|
|
% - x, y: Cartesian coordinates
|
|
% - L1, L2: Link lengths
|
|
% Output:
|
|
% - theta: Joint angles [q1; q2]
|
|
% author: Camille CONJAT, camille.conjat@ecam.fr
|
|
% date: 03/12/2023
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Calculate the term inside the acos function
|
|
term = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2);
|
|
|
|
% Check for solutions
|
|
if term > 1 || term < -1
|
|
% No solution
|
|
disp('No solution exists for the given coordinates.');
|
|
theta = [];
|
|
return;
|
|
elseif term == 1
|
|
% One solution
|
|
q1 = atan2(y, x);
|
|
q2 = 0;
|
|
elseif term == -1
|
|
% One solution
|
|
q1 = atan2(y, x);
|
|
q2 = pi;
|
|
else
|
|
% Two solutions
|
|
q1_1 = atan2(y, x) - atan2(L2 * sin(acos(term)), L1 + L2 * cos(acos(term)));
|
|
q2_1 = acos(term);
|
|
|
|
q1_2 = atan2(y, x) + atan2(L2 * sin(acos(term)), L1 + L2 * cos(acos(term)));
|
|
q2_2 = -acos(term);
|
|
|
|
% Choose the solutions based on the robot's configuration
|
|
q1 = [q1_1; q1_2];
|
|
q2 = [q2_1; q2_2];
|
|
end
|
|
|
|
% Convert angles to degrees
|
|
theta = rad2deg([q1; q2]);
|
|
end
|
|
|
|
|