-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSE3IcT.m
156 lines (154 loc) · 3.52 KB
/
SE3IcT.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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
function IcTc = SE3IcT(in1,in2)
%SE3IcT
% IcTc = SE3IcT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 9.3.
% 2024-01-16 21:47:15
I0_1 = in2(1,:);
I0_2 = in2(2,:);
I0_3 = in2(3,:);
I0_4 = in2(4,:);
I0_5 = in2(5,:);
I0_6 = in2(6,:);
I0_7 = in2(7,:);
I0_8 = in2(8,:);
I0_9 = in2(9,:);
I0_10 = in2(10,:);
T1_1_1 = in1(1);
T1_1_2 = in1(5);
T1_1_3 = in1(9);
T1_1_4 = in1(13);
T1_2_1 = in1(2);
T1_2_2 = in1(6);
T1_2_3 = in1(10);
T1_2_4 = in1(14);
T1_3_1 = in1(3);
T1_3_2 = in1(7);
T1_3_3 = in1(11);
T1_3_4 = in1(15);
t2 = T1_1_1.*T1_2_4;
t3 = T1_1_4.*T1_2_1;
t4 = T1_1_2.*T1_2_4;
t5 = T1_1_4.*T1_2_2;
t6 = T1_1_3.*T1_2_4;
t7 = T1_1_4.*T1_2_3;
t8 = T1_1_1.*T1_3_4;
t9 = T1_1_4.*T1_3_1;
t10 = T1_1_2.*T1_3_4;
t11 = T1_1_4.*T1_3_2;
t12 = T1_1_3.*T1_3_4;
t13 = T1_1_4.*T1_3_3;
t14 = T1_2_1.*T1_3_4;
t15 = T1_2_4.*T1_3_1;
t16 = T1_2_2.*T1_3_4;
t17 = T1_2_4.*T1_3_2;
t18 = T1_2_3.*T1_3_4;
t19 = T1_2_4.*T1_3_3;
t20 = I0_1.*T1_1_1;
t21 = I0_1.*T1_1_2;
t22 = I0_4.*T1_1_1;
t23 = I0_4.*T1_1_2;
t24 = I0_5.*T1_1_1;
t25 = I0_5.*T1_1_2;
t26 = I0_9.*T1_1_1;
t27 = I0_9.*T1_1_2;
t28 = I0_10.*T1_1_1;
t29 = I0_9.*T1_1_3;
t30 = I0_10.*T1_1_2;
t31 = I0_2.*T1_2_1;
t32 = I0_10.*T1_1_3;
t33 = I0_2.*T1_2_2;
t34 = I0_4.*T1_2_1;
t35 = I0_4.*T1_2_2;
t36 = I0_6.*T1_2_1;
t37 = I0_6.*T1_2_2;
t38 = I0_8.*T1_2_1;
t39 = I0_8.*T1_2_2;
t40 = I0_8.*T1_2_3;
t41 = I0_10.*T1_2_1;
t42 = I0_10.*T1_2_2;
t43 = I0_10.*T1_2_3;
t44 = I0_3.*T1_3_1;
t45 = I0_3.*T1_3_2;
t46 = I0_5.*T1_3_1;
t47 = I0_5.*T1_3_2;
t48 = I0_6.*T1_3_1;
t49 = I0_6.*T1_3_2;
t50 = I0_8.*T1_3_1;
t51 = I0_8.*T1_3_2;
t52 = I0_9.*T1_3_1;
t53 = I0_8.*T1_3_3;
t54 = I0_9.*T1_3_2;
t55 = I0_9.*T1_3_3;
t56 = -t3;
t57 = -t5;
t58 = -t7;
t59 = -t9;
t60 = -t11;
t61 = -t13;
t62 = -t15;
t63 = -t17;
t64 = -t19;
t65 = -t26;
t66 = -t27;
t67 = -t28;
t68 = -t29;
t69 = -t30;
t70 = -t32;
t71 = -t41;
t72 = -t42;
t73 = -t43;
t74 = t2+t56;
t75 = t4+t57;
t76 = t6+t58;
t77 = t8+t59;
t78 = t10+t60;
t79 = t12+t61;
t80 = t14+t62;
t81 = t16+t63;
t82 = t18+t64;
t83 = I0_7.*t74;
t84 = I0_8.*t74;
t85 = I0_7.*t75;
t86 = I0_9.*t74;
t87 = I0_8.*t75;
t88 = I0_7.*t76;
t89 = I0_9.*t75;
t90 = I0_7.*t77;
t91 = I0_8.*t77;
t92 = I0_7.*t78;
t93 = I0_8.*t78;
t94 = I0_10.*t77;
t95 = I0_7.*t79;
t96 = I0_10.*t78;
t97 = I0_7.*t80;
t98 = I0_7.*t81;
t99 = I0_9.*t80;
t100 = I0_10.*t80;
t101 = I0_7.*t82;
t102 = I0_9.*t81;
t103 = I0_10.*t81;
t104 = -t86;
t105 = -t89;
t106 = -t94;
t107 = -t96;
t108 = -t100;
t109 = -t103;
t110 = t38+t65+t83;
t111 = t39+t66+t85;
t112 = t40+t68+t88;
t113 = t50+t67+t90;
t114 = t51+t69+t92;
t115 = t53+t70+t95;
t116 = t52+t71+t97;
t117 = t54+t72+t98;
t118 = t55+t73+t101;
t119 = t24+t36+t44+t91+t99;
t120 = t25+t37+t45+t93+t102;
t121 = t22+t31+t48+t84+t108;
t122 = t23+t33+t49+t87+t109;
t123 = t20+t34+t46+t104+t106;
t124 = t21+t35+t47+t105+t107;
mt1 = [T1_1_1.*t123+T1_2_1.*t121+T1_3_1.*t119+t74.*t110+t77.*t113+t80.*t116;T1_1_2.*t124+T1_2_2.*t122+T1_3_2.*t120+t75.*t111+t78.*t114+t81.*t117;T1_1_3.*(-I0_9.*t76-I0_10.*t79+I0_1.*T1_1_3+I0_4.*T1_2_3+I0_5.*T1_3_3)+T1_2_3.*(I0_8.*t76-I0_10.*t82+I0_4.*T1_1_3+I0_2.*T1_2_3+I0_6.*T1_3_3)+T1_3_3.*(I0_8.*t79+I0_9.*t82+I0_5.*T1_1_3+I0_6.*T1_2_3+I0_3.*T1_3_3)+t76.*t112+t79.*t115+t82.*t118;T1_1_2.*t123+T1_2_2.*t121+T1_3_2.*t119+t75.*t110+t78.*t113+t81.*t116;T1_1_3.*t123+T1_2_3.*t121+T1_3_3.*t119+t76.*t110+t79.*t113+t82.*t116;T1_1_3.*t124+T1_2_3.*t122+T1_3_3.*t120+t76.*t111+t79.*t114+t82.*t117;I0_7.*T1_1_1.^2+I0_7.*T1_2_1.^2+I0_7.*T1_3_1.^2;-T1_1_2.*t118+T1_2_2.*t115-T1_3_2.*t112];
mt2 = [-T1_1_3.*t116+T1_2_3.*t113-T1_3_3.*t110;-T1_1_1.*t117+T1_2_1.*t114-T1_3_1.*t111];
IcTc = [mt1;mt2];
end