?? jacprojrts.m
字號:
function [jrt, jst]=jacprojRTS(rt, xyz, a)% symbolic projection function Jacobian a1=a(1); a2=a(2); a3=a(3); a4=a(4); %a5=a(5); qr2=rt(1); qr3=rt(2); qr4=rt(3); t1=rt(4); t2=rt(5); t3=rt(6); X=xyz(1); Y=xyz(2); Z=xyz(3); qr1=sqrt(1-(qr2^2+qr3^2+qr4^2)); jrt=[(-(2/qr1*X*qr2*qr3+(-2/qr1*Y*qr2-2*Z)*qr2+2*qr1*Y+2*qr4*X-2*qr2*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z))*a3+(-(-2/qr1*X*qr2*qr4+(2/qr1*Z*qr2-2*Y)*qr2+2*qr3*X-2*qr1*Z-2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t2)*a2+(-(2/qr1*Y*qr2*qr4-2/qr1*Z*qr2*qr3+2*qr3*Y+2*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t1)*a1,... (-((2/qr1*X*qr3-2*Z)*qr3-2*qr1*X+2*qr4*Y-2*qr3*Z-2/qr1*Y*qr3*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2))*a3+(-(-2/qr1*X*qr3*qr4+2*qr4*Z+(2*X+2/qr1*Z*qr3)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t2)*a2+(-(2/qr1*Y*qr3*qr4+(-2*X-2/qr1*Z*qr3)*qr3-2*qr3*X+2*qr1*Z+2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t1)*a1,... (-((2/qr1*X*qr4+2*Y)*qr3+(-2/qr1*Y*qr4+2*X)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2))*a3+(-((-2/qr1*X*qr4-2*Y)*qr4+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Z*qr4*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+((2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t2)*a2+(-((-2*X+2/qr1*Y*qr4)*qr4-2*qr4*X-2*qr1*Y-2/qr1*Z*qr4*qr3+2*qr2*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)+(X+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t1)*a1,... -a1/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),... -a2/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),... (-1/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-(t3+Z+(-2*qr1*X+2*qr4*Y-2*qr3*Z)*qr3+(2*qr1*Y+2*qr4*X-2*qr2*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2)*a3-(t2+Y+(2*qr1*X-2*qr4*Y)*qr4+2*qr3*qr4*Z+(2*qr3*X-2*qr1*Z-2*qr2*Y)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a2-(X+t1+(-2*qr4*X-2*qr1*Y)*qr4+(-2*qr3*X+2*qr1*Z)*qr3+(2*qr3*Y+2*qr4*Z)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a1,... ((2/qr1*X*qr2*qr4+(-2/qr1*Z*qr2+2*Y)*qr2-2*qr3*X+2*qr1*Z+2*qr2*Y)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*(-2/qr1*X*qr2*qr3+(2/qr1*Y*qr2+2*Z)*qr2-2*qr4*X-2*qr1*Y+2*qr2*Z)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr2*Z+(4*qr2^2-2+2*qr3^2+2*qr4^2)/qr1*Y+(-2*qr3*qr2-2*qr4*qr1)/qr1*X)*t2)*a4,... ((2/qr1*X*qr3*qr4-2*qr4*Z+(-2*X-2/qr1*Z*qr3)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr3+2*Z)*qr3+2*qr1*X-2*qr4*Y+2*qr3*Z+2/qr1*Y*qr3*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*(4*qr3*Z+(2*qr3*qr2-2*qr4*qr1)/qr1*Y+(-4*qr3^2+2-2*qr2^2-2*qr4^2)/qr1*X)*t2)*a4,... (((2/qr1*X*qr4+2*Y)*qr4-2*qr1*X+2*qr4*Y-2*qr3*Z-2/qr1*Z*qr4*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)-((-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*((-2/qr1*X*qr4-2*Y)*qr3+(-2*X+2/qr1*Y*qr4)*qr2)+1/((-1+2*qr3^2+2*qr2^2)*Z+(-2*qr4*qr3-2*qr1*qr2)*Y+(2*qr1*qr3-2*qr4*qr2)*X-t3)^2*((2*qr4*qr2-2*qr1*qr3)/qr1*Y+(-2*qr4*qr3-2*qr1*qr2)/qr1*X)*t2)*a4,... 0,... -a4/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2),... (-t2-Y+(-2*qr1*X+2*qr4*Y)*qr4-2*qr3*qr4*Z+(-2*qr3*X+2*qr1*Z+2*qr2*Y)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2*a4]; jst=[-(((-2*qr1*Z-2*qr1*t3)*qr4+(-2*qr1*Y-2*t2*qr1)*qr3+((2*t2-2*Y)*qr4+(2*Z-2*t3)*qr3)*qr2)*a2+(-Z-t3+(2*t3+2*Z)*qr4^2+(2*qr4*Y-2*t1*qr1+2*qr3*t3)*qr3+(2*t1*qr4-2*qr1*Y+2*qr2*Z)*qr2)*a1)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2,... ((Z+t3+(-2*t3-2*Z)*qr4^2+(-2*qr1*X-2*t2*qr4-2*qr3*Z)*qr3+(-2*qr4*X-2*t2*qr1-2*t3*qr2)*qr2)*a2+((-2*qr1*Z-2*qr1*t3)*qr4+(-2*t1+2*X)*qr4*qr3+(-2*t1*qr1-2*qr1*X+(2*t3-2*Z)*qr3)*qr2)*a1)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2,... ((-t2-Y+(-2*qr1*X+2*qr4*Y)*qr4+(2*qr4*t3+(2*t2+2*Y)*qr3)*qr3+(-2*qr1*t3+2*qr3*X+2*t2*qr2)*qr2)*a2+(-X-t1+(2*qr1*Y+2*qr4*X)*qr4+(2*qr1*t3+2*t1*qr3)*qr3+(2*qr3*Y+2*qr4*t3+(2*t1+2*X)*qr2)*qr2)*a1)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2,... 2*a4*((qr1*Z+qr1*t3)*qr4+(qr1*Y+t2*qr1)*qr3+((Y-t2)*qr4+(t3-Z)*qr3)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2,... -a4*(-Z-t3+(2*Z+2*t3)*qr4^2+(2*qr1*X+2*t2*qr4+2*qr3*Z)*qr3+(2*qr4*X+2*t2*qr1+2*t3*qr2)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2,... a4*(-t2-Y+(-2*qr1*X+2*qr4*Y)*qr4+(2*qr4*t3+(2*t2+2*Y)*qr3)*qr3+(-2*qr1*t3+2*qr3*X+2*t2*qr2)*qr2)/(-Z-t3+(2*qr1*X-2*qr4*Y+2*qr3*Z)*qr3+(-2*qr4*X-2*qr1*Y+2*qr2*Z)*qr2)^2];
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -