Matlabを使ったロボット工学のプログラムについて質問します。 下記のコードは逆運動学における2リンクロボットアームの軌道を表そうとしているプログラムです。 私がつまずいているところは、while文内の"th1,th2"の更新です。現状のプログラムだといつまでもwhileの条件が満たされず、無限ループになってしまいます。while文をの条件を満たすように書くとしたらどのようにプログラムを書けば良いのでしょうか。 しばらく私なりにも考えてみたのですが、プログラミングの考え方が未熟なのもあり思うように進みません。どなたか解決の糸口を提供していただける方、ご教示願います。 〔ソースコード〕 clc;clear; L1 = 1; L2 = 1; X = 1; Y = 1; rr_ref = [X; Y]; th1 = pi/3; th2 = -pi/3; qq = [th1;th2]; plot(X,Y,'*r', ... %手先の目標位置をプロット 'MarkerSize',10); axis([-1 2 -1 2]) err=[100;100]; while (err'*err>0.001) rr=[L1*cos(th1)+L2*cos(th1+th2); L1*sin(th1)+L2*sin(th1+th2)]; err=rr_ref-rr invJJ=(-1/(L1*L2*sin(th2)))*[-L2*cos(th1+th2) -L2*sin(th1+th2); L1*cos(th1)+L2*cos(th1+th2) L1*sin(th1)+L2*sin(th1+th2)]; qq=qq+invJJ*err; % th1,th2の更新 end
プログラミング