57 lines
1.3 KiB
Matlab
57 lines
1.3 KiB
Matlab
function [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
% function [nbSol, qi] = solveIK2LinkPlanarRobot(L1, L2, x, y)
|
|
% Task: solve Inverse Kinematics (if it exists) for a 2 link planar robot
|
|
%
|
|
% Inputs:
|
|
% - L1: length of link 1 (in m)
|
|
% - L2: length of link 1 (in m)
|
|
% - x: target x coordinate (in m)
|
|
% - y: target y coordinate (in m)
|
|
%
|
|
% Outputs:
|
|
% - nbSol: number of solutions for this IK problem
|
|
% - qi: array of joint angle values (in degrees)
|
|
%
|
|
%
|
|
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
|
% date: 09/02/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
%config
|
|
threshold = 1e-10;
|
|
|
|
cos_q2 =(x*x + y*y -(L1*L1 + L2*L2)) / (2*L1*L2);
|
|
|
|
% no solution cos_q2 >1 or <-1
|
|
if (cos_q2 > 1.0 || cos_q2 < -1.0)
|
|
nbSol = 0;
|
|
qi = [];
|
|
|
|
% one solution cos_q2=1 or -1
|
|
elseif (abs(cos_q2-1.0) < threshold)
|
|
nbSol = 1;
|
|
q2 = 0;
|
|
q1 = atan2(y,x);
|
|
qi = [rad2deg(q1) rad2deg(q2)];
|
|
elseif (abs(cos_q2+1.0) < threshold)
|
|
nbSol = 1;
|
|
q2 = pi;
|
|
q1 = atan2(y,x)
|
|
qi = [rad2deg(q1) rad2deg(q2)]
|
|
|
|
% two solutions -1< cos_q2 < 1
|
|
elseif (cos_q2 > -1.0 && cos_q2 < 1.0)
|
|
nbSol = 2;
|
|
q2_1 = acos(cos_q2);
|
|
q2_2 = 2*pi-acos(cos_q2);
|
|
|
|
q1_1 = atan2(y,x)-atan2(L2*sin(q2_1),(L1+L2*cos_q2));
|
|
q1_2 = atan2(y,x)+atan2(L2*sin(q2_1),(L1+L2*cos_q2));
|
|
|
|
qi = [rad2deg(q1_1) rad2deg(q2_1); rad2deg(q1_2) rad2deg(q2_2)];
|
|
end
|
|
|
|
|
|
|