|
function [q1 q2 q1_ q2_] = MyIK(L1, L2, x, y)
|
|
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 |