Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- %% Dane
- pyp=1
- pxp=1
- pyk=2
- pxk=-1
- tk=10;
- tp=0
- L1=5.8;
- L2=1;
- VP=[-1;0]
- VK=[0;1]
- %% p na q
- q2p=acos((pyp^2+pxp^2-L1^2-L2^2)/(2*L1*L2))
- q1p=asin((L2*sin(q2p))/(sqrt(pxp^2+pyp^2))+atan(pxp/pyp))
- QP=[q1p;q2p]
- q2k=acos((pyk^2+pxk^2-L1^2-L2^2)/(2*L1*L2))
- q1k=asin((L2*sin(q2k))/(sqrt(pxk^2+pyk^2))+atan(pxk/pyk))
- QK=[q1k;q2k]
- %% Jakobiany
- JP=[-L1*sin(q1p)-L2*sin(q1p+q2p) -L2*sin(q1p+q2p); L1*cos(q1p)+L2*cos(q1p+q2p) L2*cos(q1p+q2p)]
- QPd=inv(JP)*VP
- %% Wspolczynniki
- a0=QP;
- a1=QPd;
- a2=3/tk^2*(QK-QP)
- a3=-2/tk^3*(QK-QP)
- t=0:0.01:tk;
- y=a0+a1*t+a2*t.^2+a3*t.^3;
- figure(1)
- plot(t,y);
- %% Trajektoria
- figure(2)
- for t=0:0.01:tk
- y=a0+a1*t+a2*t.^2+a3*t.^3;
- PX=L1*cos(y(1,1))+L2*cos(y(1,1)+y(2,1));
- PY=L1*sin(y(1,1))+L2*sin(y(1,1)+y(2,1));
- plot(PX,PY,'r.')
- hold on
- end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement