32 lines
1.1 KiB
Matlab
32 lines
1.1 KiB
Matlab
function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
|
|
%
|
|
% Task: returns the two possible solutions of q1 q2 coordinate in the joint space
|
|
% from a given point in the x y cartesian space
|
|
% using geometry rules since we know that we have two links only of length L1 and L2
|
|
%
|
|
%
|
|
% Inputs:
|
|
%-L1: length of the first link of the 2d planar 2 link robot arm
|
|
%-L2: length of the second link of the 2d planar 2 link robot arm
|
|
%-x: x coordinate of the given point
|
|
%-y: x coordinate of the given point
|
|
%
|
|
% Outputs:
|
|
%-q1: q1 coordinate of the given point (first solution)
|
|
%-q2: q2 coordinate of the given point (first solution)
|
|
%-q1_: q1 coordinate of the given point (second solution)
|
|
%-q2_: q2 coordinate of the given point (second solution)
|
|
%
|
|
%
|
|
% Thomas OLIVE (thomas.olive@ecam.fr)
|
|
% 18/12/2021
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
q1 = acosd(x/sqrt(x^2+y^2)) - acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
|
|
q2 = 180 - acosd( (L1^2+L2^2-(x^2+y^2)) / (2*L1*L2));
|
|
|
|
q1_ = q1 + 2*acosd( (-L2^2+x^2+y^2+L1^2) / (2*L1*sqrt(x^2+y^2)));
|
|
q2_ = -q2;
|
|
endfunction |