-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathgetAnaliticJacobian.m
7 lines (5 loc) · 5.66 KB
/
getAnaliticJacobian.m
1
2
3
4
5
6
function J = getAnaliticJacobian(q)
J = [ (21*sin(q(1))*sin(q(2)))/50 - (21*cos(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 + (21*sin(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 - (2*sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*cos(q(4))*sin(q(1))*sin(q(2)))/5, -(cos(q(1))*(42*cos(q(2)) + 40*cos(q(2))*cos(q(4)) + 21*cos(q(2))*cos(q(4))*cos(q(6)) + 40*cos(q(3))*sin(q(2))*sin(q(4)) + 21*cos(q(3))*cos(q(6))*sin(q(2))*sin(q(4)) + 21*cos(q(2))*cos(q(5))*sin(q(4))*sin(q(6)) + 21*sin(q(2))*sin(q(3))*sin(q(5))*sin(q(6)) - 21*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(6))))/100, - (2*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3))))/5 - (21*sin(q(6))*(sin(q(5))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(4))*cos(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 - (21*cos(q(6))*sin(q(4))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3))))/100, (2*cos(q(1))*sin(q(2))*sin(q(4)))/5 - (2*cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (21*cos(q(5))*sin(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*cos(q(6))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))))/100, -(21*sin(q(6))*(sin(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) - cos(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100, (21*sin(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 + (21*cos(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100, 0;
(21*sin(q(6))*(cos(q(5))*(cos(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) - cos(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(3))*sin(q(1)) + cos(q(1))*cos(q(2))*sin(q(3)))))/100 - (21*cos(q(6))*(sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))) + cos(q(1))*cos(q(4))*sin(q(2))))/100 - (21*cos(q(1))*sin(q(2)))/50 - (2*sin(q(4))*(sin(q(1))*sin(q(3)) - cos(q(1))*cos(q(2))*cos(q(3))))/5 - (2*cos(q(1))*cos(q(4))*sin(q(2)))/5, -(sin(q(1))*(42*cos(q(2)) + 40*cos(q(2))*cos(q(4)) + 21*cos(q(2))*cos(q(4))*cos(q(6)) + 40*cos(q(3))*sin(q(2))*sin(q(4)) + 21*cos(q(3))*cos(q(6))*sin(q(2))*sin(q(4)) + 21*cos(q(2))*cos(q(5))*sin(q(4))*sin(q(6)) + 21*sin(q(2))*sin(q(3))*sin(q(5))*sin(q(6)) - 21*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(6))))/100, (2*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3))))/5 + (21*sin(q(6))*(sin(q(5))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*cos(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100 + (21*cos(q(6))*sin(q(4))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3))))/100, (21*cos(q(6))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))))/100 + (2*cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))))/5 + (2*sin(q(1))*sin(q(2))*sin(q(4)))/5 + (21*cos(q(5))*sin(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100, (21*sin(q(6))*(sin(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) - cos(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100, - (21*sin(q(6))*(sin(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) - cos(q(4))*sin(q(1))*sin(q(2))))/100 - (21*cos(q(6))*(cos(q(5))*(cos(q(4))*(cos(q(1))*sin(q(3)) + cos(q(2))*cos(q(3))*sin(q(1))) + sin(q(1))*sin(q(2))*sin(q(4))) + sin(q(5))*(cos(q(1))*cos(q(3)) - cos(q(2))*sin(q(1))*sin(q(3)))))/100, 0;
0, (2*cos(q(2))*cos(q(3))*sin(q(4)))/5 - (2*cos(q(4))*sin(q(2)))/5 - (21*sin(q(6))*(cos(q(5))*(sin(q(2))*sin(q(4)) + cos(q(2))*cos(q(3))*cos(q(4))) - cos(q(2))*sin(q(3))*sin(q(5))))/100 - (21*cos(q(6))*(cos(q(4))*sin(q(2)) - cos(q(2))*cos(q(3))*sin(q(4))))/100 - (21*sin(q(2)))/50, -(sin(q(2))*(40*sin(q(3))*sin(q(4)) + 21*cos(q(6))*sin(q(3))*sin(q(4)) - 21*cos(q(3))*sin(q(5))*sin(q(6)) - 21*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(6))))/100, (21*cos(q(5))*sin(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100 - (21*cos(q(6))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))))/100 - (2*cos(q(2))*sin(q(4)))/5 + (2*cos(q(3))*cos(q(4))*sin(q(2)))/5, -(21*sin(q(6))*(sin(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) - cos(q(5))*sin(q(2))*sin(q(3))))/100, (21*cos(q(6))*(cos(q(5))*(cos(q(2))*sin(q(4)) - cos(q(3))*cos(q(4))*sin(q(2))) + sin(q(2))*sin(q(3))*sin(q(5))))/100 - (21*sin(q(6))*(cos(q(2))*cos(q(4)) + cos(q(3))*sin(q(2))*sin(q(4))))/100, 0];
end