function arm_inv_arm_angle
format compact;
clear all;
close all;

state_servo1=struct('angle',0,'length',0.085);
state_servo2=struct('angle',0,'length',0.085);

f = figure('Position',[1300,400,600,600]);
ax = axes('Parent',f,'position',[0.1 0.1  0.8 0.8]);
h=plot(0,0);hold on;pbaspect([1 1 1]);  %表示アスペクト比を1:1
axis([-0.2,0.2,-0.2,0.2]);

% 角度で入力
% state_servo1.angle=deg2rad(-22.9094);
% state_servo2.angle=deg2rad(-68.5216);
% plot_arm(f,state_servo1,state_servo2)

% 逆運動学で計算してみる
% % ref=gen_servo_angle(0.2, 0.0)
% state_servo1.angle =ref(1);
% state_servo2.angle =ref(2);
% plot_arm(f,state_servo1,state_servo2)

% 逆運動学で直線を出す
y_ref=-0.05:0.005:0.05;
x_ref=ones(1,length(y_ref)).*0.15;

% 逆運動学で円を出す
% th=linspace(0,2*pi,100);
% R=0.01;
% x_ref=sin(th).*R +0.15;
% y_ref=cos(th).*R +0;

% アニメーション実行
t=0:Ts:(length(x_ref)-1)*Ts;
data_servo1_anlge=0;data_servo2_anlge=0;
data_servo1_anlge_J=0;data_servo2_anlge_J=0;
JacobianTheta=0;last_p=0;  %ヤコビ行列で計算するとき
for i=1:length(x_ref)
%     解析解を用いた場合
    ref=gen_servo_angle(x_ref(i),y_ref(i));

%     ヤコビ行列を用いた場合
    if i==1
        last_p=[x_ref(1);y_ref(1)];
        JacobianTheta=[ref(1);ref(2)];
    else
        J=JacobianMatrix(state_servo1.angle,state_servo2.angle,...
            state_servo1.length,state_servo2.length);
        delta_p=[x_ref(i);y_ref(i)]-last_p;
        last_p = [x_ref(i);y_ref(i)];
        JacobianTheta = JacobianTheta + J\ delta_p;
    end

%     ヤコビ行列でやる場合は下のコメントアウトを解除
%     ref = JacobianTheta';

    state_servo1.angle =ref(1);
    state_servo2.angle =ref(2);

    data_servo1_anlge(i)=ref(1);
    data_servo2_anlge(i)=ref(2);
    data_servo1_anlge_J(i)=JacobianTheta(1);
    data_servo2_anlge_J(i)=JacobianTheta(2);

    plot_arm(f,state_servo1,state_servo2)
    pause(20/1000);
end

% 角度情報を見てみる
figure();
plot(t,rad2deg(data_servo1_anlge),t,rad2deg(data_servo2_anlge),...
    t,rad2deg(data_servo1_anlge_J),t,rad2deg(data_servo2_anlge_J));
legend('servo1','servo2','servo1_J','servo2_J')

% ヤコビ行列を求める
function J=JacobianMatrix(theta1,theta2,L1,L2)
    J=[-L1 * sin(theta1)-L2*sin(theta1+theta2) ,-L2*sin(theta1+theta2);
        L1*cos(theta1)+L2*cos(theta1+theta2), L2*cos(theta1+theta2)];
end

function plot_arm(f,servo1,servo2)
figure(f);
cla; %この行をコメントアウトするとアームのステップごとの位置が見れる
% arm 1st
plot([0.0 , servo1.length * cos(servo1.angle)],...
    [0.0 ,servo1.length * sin(servo1.angle)],'b');
% arm 2nd
plot([servo1.length * cos(servo1.angle) ,...
        servo1.length * cos(servo1.angle)...
        + servo2.length * cos(servo1.angle + servo2.angle)],...
    [servo1.length * sin(servo1.angle) ,...
        servo1.length * sin(servo1.angle)...
        + servo2.length * sin(servo1.angle + servo2.angle)],'r');
end

function theta=gen_servo_angle(x,y)
    L1=state_servo1.length;
    L2=state_servo2.length;

%     ここで+-の候補が出てくる．
    theta(1)=atan2(y,x)...
        -acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
%         +acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
    theta(2)=-theta(1) + atan2((y-L1*sin(theta(1))) , (x-L1*cos(theta(1))));
end

end