-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSE3IcT2.m
126 lines (124 loc) · 2.89 KB
/
SE3IcT2.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
function IcTc2 = SE3IcT2(in1,in2,in3)
%SE3IcT2
% IcTc2 = SE3IcT2(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 9.3.
% 2024-01-16 21:48:01
I0_1 = in3(1,:);
I0_2 = in3(2,:);
I0_3 = in3(3,:);
I0_4 = in3(4,:);
I0_5 = in3(5,:);
I0_6 = in3(6,:);
I0_7 = in3(7,:);
I0_8 = in3(8,:);
I0_9 = in3(9,:);
I0_10 = in3(10,:);
R1_1 = in1(1);
R1_2 = in1(4);
R1_3 = in1(7);
R2_1 = in1(2);
R2_2 = in1(5);
R2_3 = in1(8);
R3_1 = in1(3);
R3_2 = in1(6);
R3_3 = in1(9);
phR1_1 = in2(1);
phR1_2 = in2(4);
phR1_3 = in2(7);
phR2_1 = in2(2);
phR2_2 = in2(5);
phR2_3 = in2(8);
phR3_1 = in2(3);
phR3_2 = in2(6);
phR3_3 = in2(9);
t2 = I0_7.*phR1_1;
t3 = I0_7.*phR1_2;
t4 = I0_7.*phR1_3;
t5 = I0_9.*phR1_1;
t6 = I0_9.*phR1_2;
t7 = I0_10.*phR1_1;
t8 = I0_10.*phR1_2;
t9 = I0_7.*phR2_1;
t10 = I0_7.*phR2_2;
t11 = I0_8.*phR2_1;
t12 = I0_7.*phR2_3;
t13 = I0_8.*phR2_2;
t14 = I0_10.*phR2_1;
t15 = I0_10.*phR2_2;
t16 = I0_7.*phR3_1;
t17 = I0_7.*phR3_2;
t18 = I0_8.*phR3_1;
t19 = I0_7.*phR3_3;
t20 = I0_8.*phR3_2;
t21 = I0_9.*phR3_1;
t22 = I0_9.*phR3_2;
t23 = I0_1.*R1_1;
t24 = I0_1.*R1_2;
t25 = I0_4.*R1_1;
t26 = I0_4.*R1_2;
t27 = I0_5.*R1_1;
t28 = I0_5.*R1_2;
t29 = I0_9.*R1_1;
t30 = I0_9.*R1_2;
t31 = I0_10.*R1_1;
t32 = I0_9.*R1_3;
t33 = I0_10.*R1_2;
t34 = I0_2.*R2_1;
t35 = I0_10.*R1_3;
t36 = I0_2.*R2_2;
t37 = I0_4.*R2_1;
t38 = I0_4.*R2_2;
t39 = I0_6.*R2_1;
t40 = I0_6.*R2_2;
t41 = I0_8.*R2_1;
t42 = I0_8.*R2_2;
t43 = I0_8.*R2_3;
t44 = I0_10.*R2_1;
t45 = I0_10.*R2_2;
t46 = I0_10.*R2_3;
t47 = I0_3.*R3_1;
t48 = I0_3.*R3_2;
t49 = I0_5.*R3_1;
t50 = I0_5.*R3_2;
t51 = I0_6.*R3_1;
t52 = I0_6.*R3_2;
t53 = I0_8.*R3_1;
t54 = I0_8.*R3_2;
t55 = I0_9.*R3_1;
t56 = I0_8.*R3_3;
t57 = I0_9.*R3_2;
t58 = I0_9.*R3_3;
t59 = -t5;
t60 = -t6;
t61 = -t14;
t62 = -t15;
t63 = -t18;
t64 = -t20;
t65 = -t31;
t66 = -t33;
t67 = -t35;
t68 = -t41;
t69 = -t42;
t70 = -t43;
t71 = -t55;
t72 = -t57;
t73 = -t58;
t74 = t16+t29+t68;
t75 = t9+t53+t65;
t76 = t2+t44+t71;
t77 = t17+t30+t69;
t78 = t10+t54+t66;
t79 = t3+t45+t72;
t80 = t19+t32+t70;
t81 = t12+t56+t67;
t82 = t4+t46+t73;
t83 = t11+t27+t39+t47+t59;
t84 = t13+t28+t40+t48+t60;
t85 = t7+t25+t34+t51+t63;
t86 = t8+t26+t36+t52+t64;
t87 = t21+t23+t37+t49+t61;
t88 = t22+t24+t38+t50+t62;
mt1 = [R1_1.*t87+R2_1.*t85+R3_1.*t83+phR1_1.*t76+phR2_1.*t75+phR3_1.*t74;R1_2.*t88+R2_2.*t86+R3_2.*t84+phR1_2.*t79+phR2_2.*t78+phR3_2.*t77;R1_3.*(-I0_10.*phR2_3+I0_9.*phR3_3+I0_1.*R1_3+I0_4.*R2_3+I0_5.*R3_3)+R2_3.*(I0_10.*phR1_3-I0_8.*phR3_3+I0_4.*R1_3+I0_2.*R2_3+I0_6.*R3_3)+R3_3.*(-I0_9.*phR1_3+I0_8.*phR2_3+I0_5.*R1_3+I0_6.*R2_3+I0_3.*R3_3)+phR1_3.*t82+phR2_3.*t81+phR3_3.*t80;R1_2.*t87+R2_2.*t85+R3_2.*t83+phR1_2.*t76+phR2_2.*t75+phR3_2.*t74;R1_3.*t87+R2_3.*t85+R3_3.*t83+phR1_3.*t76+phR2_3.*t75+phR3_3.*t74;R1_3.*t88+R2_3.*t86+R3_3.*t84+phR1_3.*t79+phR2_3.*t78+phR3_3.*t77;I0_7.*R1_1.^2+I0_7.*R2_1.^2+I0_7.*R3_1.^2;R1_2.*t82+R2_2.*t81+R3_2.*t80];
mt2 = [R1_3.*t76+R2_3.*t75+R3_3.*t74;R1_1.*t79+R2_1.*t78+R3_1.*t77];
IcTc2 = [mt1;mt2];
end