theta1を0から90度まで振る
clear; close all; syms L1 L2 L3; syms th1 th2 th3; x1 = L1*cos(th1); y1 = L1*sin(th1); x2 = L1*cos(th1)+L2*cos(th1+th2); y2 = L1*sin(th1)+L2*sin(th1+th2); x3 = L1*cos(th1)+L2*cos(th1+th2)+L3*cos(th1+th2+th3); y3 = L1*sin(th1)+L2*sin(th1+th2)+L3*sin(th1+th2+th3); J3 = [diff(x3, th1) diff(x3, th2) diff(x3, th3); diff(y3, th1) diff(y3, th2) diff(y3, th3)]; l1 = 1; l2 = 0.6; l3 = 0.8; figure(); for t1 = linspace(0,pi/2,10) t2 = 0; t3 = 0; x1i = vpa(subs(x1, ... [L1 th1], ... [l1 t1])); y1i = vpa(subs(y1, ... [L1 th1], ... [l1 t1])); x2i = vpa(subs(x2, ... [L1 L2 th1 th2], ... [l1 l2 t1 t2])); y2i = vpa(subs(y2, ... [L1 L2 th1 th2], ... [l1 l2 t1 t2])); x3i = vpa(subs(x3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); y3i = vpa(subs(y3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); cla; hold on; plot([0 x1i],[0 y1i],'k-'); plot([x1i x2i],[y1i y2i],'k-'); plot([x2i x3i],[y2i y3i],'k-'); plot(0,0,'ko','LineWidth',3); plot(x1i,y1i,'bo','LineWidth',3); plot(x2i,y2i,'go','LineWidth',3); plot(x3i,y3i,'ro','LineWidth',3); drawnow(); xlim([-0.2,3]); ylim([-0.2,3]); pause(0.5); end
View post on imgur.comimgur.com
