% 12/4の進捗報告の図1で，目標を真上姿勢にしたもの
%xi=0.07; %初期値 
%yi=0.08;
%zi=0.16;
% 初期姿勢を前向き真下屈曲にしたもの
xi=0.11; %初期値 
yi=0;
zi=0.09;
xf=0; %最終値
yf=0;
zf=0.2;
l2=0.11; 
l3=0.09;

A=xi/cos(atan(yi/xi));
B=zi;
gamma=acos(((A^2)+(B^2)-(l2^2)-(l3^2))/(2*l2*l3));
C=atan((l3*sin(gamma))/(l2+(l3*cos(gamma))));

% ３関節の初期角度
%R1(1)=atan(yi/xi);
%R2(1)=asin(B/sqrt(A^2+B^2))-C;
%R3(1)=gamma;
R1(1)=0;
R2(1)=pi/2; % 90°
R3(1)=-pi/2; % -90°

% 始点終点間を1000分割
X=(xf-xi)/999;
Y=(yf-yi)/999;
Z=(zf-zi)/999;

for i=1:999 % ループ回数
% ヤコビ行列の各要素
J11=-sin(R1(i))*(l2*sin(R2(i))+l3*cos(R2(i)+R3(i)));
J12=cos(R1(i))*(l2*cos(R2(i))+l3*cos(R2(i)+R3(i)));
J13=l3*cos(R1(i))*cos(R2(i)+R3(i));
J21=cos(R1(i))*(l2*sin(R2(i))+l3*cos(R2(i)+R3(i)));
J22=sin(R1(i))*(l2*cos(R2(i))+l3*cos(R2(i)+R3(i)));
J23=l3*sin(R1(i))*cos(R2(i)+R3(i));
J31=0;
J32=-l2*sin(R2(i))-l3*sin(R2(i)+R3(i));
J33=-l3*sin(R2(i)+R3(i));

% ヤコビ行列の逆行列
detJ=J11*J22*J33+J21*J32*J13+J31*J12*J23-J11*J32*J23-J21*J12*J33-J31*J22*J13;
A11=(J22*J33-J32*J23)/detJ;
A12=(-J12*J33+J32*J13)/detJ;
A13=(J12*J23-J22*J13)/detJ;
A21=(-J21*J33+J31*J23)/detJ;
A22=(J11*J33-J31*J13)/detJ;
A23=(-J11*J23+J21*J13)/detJ;
A31=(J21*J32-J31*J22)/detJ;
A32=(-J11*J32+J31*J12)/detJ;
A33=(J11*J22-J21*J12)/detJ;

% 現在の手先位置
x(i)=(l2*sin(R2(i))+l3*sin(R2(i)+R3(i)))*cos(R1(i));
y(i)=(l2*sin(R2(i))+l3*sin(R2(i)+R3(i)))*sin(R1(i));
z(i)=l2*cos(R2(i))+l3*cos(R2(i)+R3(i));

% 1ループ次の各関節の角度
R1(i+1)=R1(i)+A11*X+A12*Y+A13*Z;
R2(i+1)=R2(i)+A21*X+A22*Y+A23*Z;
R3(i+1)=R3(i)+A31*X+A32*Y+A33*Z;




end

