1function [Tj Tm Tp dTj dTm dTp ddTm M] = Tmatrices(q, dq, p_in)
2
3origin_x = q(1);
4origin_dx = dq(1);
5origin_y = q(2);
6origin_dy = dq(2);
7origin_q = q(3);
8origin_dq = dq(3);
9lefthip_q = q(4);
10lefthip_dq = dq(4);
11leftknee_q = q(5);
12leftknee_dq = dq(5);
13leftankle_q = q(6);
14leftankle_dq = dq(6);
15righthip_q = q(7);
16righthip_dq = dq(7);
17rightknee_q = q(8);
18rightknee_dq = dq(8);
19rightankle_q = q(9);
20rightankle_dq = dq(9);
21l_origin = p_in(1, 1);
22theta_origin = p_in(1, 2);
23l_lefthip = p_in(2, 1);
24theta_lefthip = p_in(2, 2);
25l_leftknee = p_in(3, 1);
26theta_leftknee = p_in(3, 2);
27l_leftankle = p_in(4, 1);
28theta_leftankle = p_in(4, 2);
29l_righthip = p_in(5, 1);
30theta_righthip = p_in(5, 2);
31l_rightknee = p_in(6, 1);
32theta_rightknee = p_in(6, 2);
33l_rightankle = p_in(7, 1);
34theta_rightankle = p_in(7, 2);
35l_torso = p_in(8, 1);
36theta_torso = p_in(8, 2);
37l_leftthigh = p_in(9, 1);
38theta_leftthigh = p_in(9, 2);
39l_leftshank = p_in(10, 1);
40theta_leftshank = p_in(10, 2);
41l_leftfoot = p_in(11, 1);
42theta_leftfoot = p_in(11, 2);
43l_rightthigh = p_in(12, 1);
44theta_rightthigh = p_in(12, 2);
45l_rightshank = p_in(13, 1);
46theta_rightshank = p_in(13, 2);
47l_rightfoot = p_in(14, 1);
48theta_rightfoot = p_in(14, 2);
49l_leftfoot1 = p_in(15, 1);
50theta_leftfoot1 = p_in(15, 2);
51l_leftfoot2 = p_in(16, 1);
52theta_leftfoot2 = p_in(16, 2);
53l_leftfoot3 = p_in(17, 1);
54theta_leftfoot3 = p_in(17, 2);
55l_leftfoot4 = p_in(18, 1);
56theta_leftfoot4 = p_in(18, 2);
57l_leftfoot5 = p_in(19, 1);
58theta_leftfoot5 = p_in(19, 2);
59l_leftfoot6 = p_in(20, 1);
60theta_leftfoot6 = p_in(20, 2);
61l_rightfoot1 = p_in(21, 1);
62theta_rightfoot1 = p_in(21, 2);
63l_rightfoot2 = p_in(22, 1);
64theta_rightfoot2 = p_in(22, 2);
65l_rightfoot3 = p_in(23, 1);
66theta_rightfoot3 = p_in(23, 2);
67l_rightfoot4 = p_in(24, 1);
68theta_rightfoot4 = p_in(24, 2);
69l_rightfoot5 = p_in(25, 1);
70theta_rightfoot5 = p_in(25, 2);
71l_rightfoot6 = p_in(26, 1);
72theta_rightfoot6 = p_in(26, 2);
73Tj = zeros(21,1);
74Tm = zeros(21,1);
75Tp = zeros(36,1);
76dTj = zeros(21, 9);
77dTm = zeros(21, 9);
78dTp = zeros(36, 9);
79ddTm = zeros(21, 9);
80origin = [l_origin * -sin(theta_origin) + origin_x; l_origin * cos(theta_origin) + origin_y; 0];
81lefthip = [l_lefthip * -sin(theta_lefthip + origin_q); l_lefthip * cos(theta_lefthip + origin_q); 0];
82leftknee = [l_leftknee * -sin(theta_leftknee + lefthip_q); l_leftknee * cos(theta_leftknee + lefthip_q); 0];
83leftankle = [l_leftankle * -sin(theta_leftankle + leftknee_q); l_leftankle * cos(theta_leftankle + leftknee_q); 0];
84righthip = [l_righthip * -sin(theta_righthip + origin_q); l_righthip * cos(theta_righthip + origin_q); 0];
85rightknee = [l_rightknee * -sin(theta_rightknee + righthip_q); l_rightknee * cos(theta_rightknee + righthip_q); 0];
86rightankle = [l_rightankle * -sin(theta_rightankle + rightknee_q); l_rightankle * cos(theta_rightankle + rightknee_q); 0];
87torso = [l_torso * -sin(theta_torso + origin_q); l_torso * cos(theta_torso + origin_q); origin_q];
88leftthigh = [l_leftthigh * -sin(theta_leftthigh + lefthip_q); l_leftthigh * cos(theta_leftthigh + lefthip_q); lefthip_q];
89leftshank = [l_leftshank * -sin(theta_leftshank + leftknee_q); l_leftshank * cos(theta_leftshank + leftknee_q); leftknee_q];
90leftfoot = [l_leftfoot * -sin(theta_leftfoot + leftankle_q); l_leftfoot * cos(theta_leftfoot + leftankle_q); leftankle_q];
91rightthigh = [l_rightthigh * -sin(theta_rightthigh + righthip_q); l_rightthigh * cos(theta_rightthigh + righthip_q); righthip_q];
92rightshank = [l_rightshank * -sin(theta_rightshank + rightknee_q); l_rightshank * cos(theta_rightshank + rightknee_q); rightknee_q];
93rightfoot = [l_rightfoot * -sin(theta_rightfoot + rightankle_q); l_rightfoot * cos(theta_rightfoot + rightankle_q); rightankle_q];
94leftfoot1 = [l_leftfoot1 * -sin(theta_leftfoot1 + leftankle_q); l_leftfoot1 * cos(theta_leftfoot1 + leftankle_q); leftankle_q];
95leftfoot2 = [l_leftfoot2 * -sin(theta_leftfoot2 + leftankle_q); l_leftfoot2 * cos(theta_leftfoot2 + leftankle_q); leftankle_q];
96leftfoot3 = [l_leftfoot3 * -sin(theta_leftfoot3 + leftankle_q); l_leftfoot3 * cos(theta_leftfoot3 + leftankle_q); leftankle_q];
97leftfoot4 = [l_leftfoot4 * -sin(theta_leftfoot4 + leftankle_q); l_leftfoot4 * cos(theta_leftfoot4 + leftankle_q); leftankle_q];
98leftfoot5 = [l_leftfoot5 * -sin(theta_leftfoot5 + leftankle_q); l_leftfoot5 * cos(theta_leftfoot5 + leftankle_q); leftankle_q];
99leftfoot6 = [l_leftfoot6 * -sin(theta_leftfoot6 + leftankle_q); l_leftfoot6 * cos(theta_leftfoot6 + leftankle_q); leftankle_q];
100rightfoot1 = [l_rightfoot1 * -sin(theta_rightfoot1 + rightankle_q); l_rightfoot1 * cos(theta_rightfoot1 + rightankle_q); rightankle_q];
101rightfoot2 = [l_rightfoot2 * -sin(theta_rightfoot2 + rightankle_q); l_rightfoot2 * cos(theta_rightfoot2 + rightankle_q); rightankle_q];
102rightfoot3 = [l_rightfoot3 * -sin(theta_rightfoot3 + rightankle_q); l_rightfoot3 * cos(theta_rightfoot3 + rightankle_q); rightankle_q];
103rightfoot4 = [l_rightfoot4 * -sin(theta_rightfoot4 + rightankle_q); l_rightfoot4 * cos(theta_rightfoot4 + rightankle_q); rightankle_q];
104rightfoot5 = [l_rightfoot5 * -sin(theta_rightfoot5 + rightankle_q); l_rightfoot5 * cos(theta_rightfoot5 + rightankle_q); rightankle_q];
105rightfoot6 = [l_rightfoot6 * -sin(theta_rightfoot6 + rightankle_q); l_rightfoot6 * cos(theta_rightfoot6 + rightankle_q); rightankle_q];
106Tj(1:3) = origin;
107Tj(4:6) = lefthip + origin;
108Tj(7:9) = leftknee + lefthip + origin;
109Tj(10:12) = leftankle + leftknee + lefthip + origin;
110Tj(13:15) = righthip + origin;
111Tj(16:18) = rightknee + righthip + origin;
112Tj(19:21) = rightankle + rightknee + righthip + origin;
113Tm(1:3) = torso + origin;
114Tm(4:6) = leftthigh + lefthip + origin;
115Tm(7:9) = leftshank + leftknee + lefthip + origin;
116Tm(10:12) = leftfoot + leftankle + leftknee + lefthip + origin;
117Tm(13:15) = rightthigh + righthip + origin;
118Tm(16:18) = rightshank + rightknee + righthip + origin;
119Tm(19:21) = rightfoot + rightankle + rightknee + righthip + origin;
120Tp(1:3) = leftfoot1 + leftankle + leftknee + lefthip + origin;
121Tp(4:6) = leftfoot2 + leftankle + leftknee + lefthip + origin;
122Tp(7:9) = leftfoot3 + leftankle + leftknee + lefthip + origin;
123Tp(10:12) = leftfoot4 + leftankle + leftknee + lefthip + origin;
124Tp(13:15) = leftfoot5 + leftankle + leftknee + lefthip + origin;
125Tp(16:18) = leftfoot6 + leftankle + leftknee + lefthip + origin;
126Tp(19:21) = rightfoot1 + rightankle + rightknee + righthip + origin;
127Tp(22:24) = rightfoot2 + rightankle + rightknee + righthip + origin;
128Tp(25:27) = rightfoot3 + rightankle + rightknee + righthip + origin;
129Tp(28:30) = rightfoot4 + rightankle + rightknee + righthip + origin;
130Tp(31:33) = rightfoot5 + rightankle + rightknee + righthip + origin;
131Tp(34:36) = rightfoot6 + rightankle + rightknee + righthip + origin;
132lefthip_origin_x = [1; 0; 0];
133lefthip_origin_y = [0; 1; 0];
134lefthip_origin_q = [l_lefthip * -cos(theta_lefthip + origin_q); l_lefthip * -sin(theta_lefthip + origin_q); 1];
135leftknee_lefthip_q = [l_leftknee * -cos(theta_leftknee + lefthip_q); l_leftknee * -sin(theta_leftknee + lefthip_q); 0];
136leftankle_leftknee_q = [l_leftankle * -cos(theta_leftankle + leftknee_q); l_leftankle * -sin(theta_leftankle + leftknee_q); 0];
137righthip_origin_x = [1; 0; 0];
138righthip_origin_y = [0; 1; 0];
139righthip_origin_q = [l_righthip * -cos(theta_righthip + origin_q); l_righthip * -sin(theta_righthip + origin_q); 1];
140rightknee_righthip_q = [l_rightknee * -cos(theta_rightknee + righthip_q); l_rightknee * -sin(theta_rightknee + righthip_q); 0];
141rightankle_rightknee_q = [l_rightankle * -cos(theta_rightankle + rightknee_q); l_rightankle * -sin(theta_rightankle + rightknee_q); 0];
142torso_origin_x = [1; 0; 0];
143torso_origin_y = [0; 1; 0];
144torso_origin_q = [l_torso * -cos(theta_torso + origin_q); l_torso * -sin(theta_torso + origin_q); 1];
145leftthigh_lefthip_q = [l_leftthigh * -cos(theta_leftthigh + lefthip_q); l_leftthigh * -sin(theta_leftthigh + lefthip_q); 1];
146leftshank_leftknee_q = [l_leftshank * -cos(theta_leftshank + leftknee_q); l_leftshank * -sin(theta_leftshank + leftknee_q); 1];
147leftfoot_leftankle_q = [l_leftfoot * -cos(theta_leftfoot + leftankle_q); l_leftfoot * -sin(theta_leftfoot + leftankle_q); 1];
148rightthigh_righthip_q = [l_rightthigh * -cos(theta_rightthigh + righthip_q); l_rightthigh * -sin(theta_rightthigh + righthip_q); 1];
149rightshank_rightknee_q = [l_rightshank * -cos(theta_rightshank + rightknee_q); l_rightshank * -sin(theta_rightshank + rightknee_q); 1];
150rightfoot_rightankle_q = [l_rightfoot * -cos(theta_rightfoot + rightankle_q); l_rightfoot * -sin(theta_rightfoot + rightankle_q); 1];
151leftfoot1_leftankle_q = [l_leftfoot1 * -cos(theta_leftfoot1 + leftankle_q); l_leftfoot1 * -sin(theta_leftfoot1 + leftankle_q); 1];
152leftfoot2_leftankle_q = [l_leftfoot2 * -cos(theta_leftfoot2 + leftankle_q); l_leftfoot2 * -sin(theta_leftfoot2 + leftankle_q); 1];
153leftfoot3_leftankle_q = [l_leftfoot3 * -cos(theta_leftfoot3 + leftankle_q); l_leftfoot3 * -sin(theta_leftfoot3 + leftankle_q); 1];
154leftfoot4_leftankle_q = [l_leftfoot4 * -cos(theta_leftfoot4 + leftankle_q); l_leftfoot4 * -sin(theta_leftfoot4 + leftankle_q); 1];
155leftfoot5_leftankle_q = [l_leftfoot5 * -cos(theta_leftfoot5 + leftankle_q); l_leftfoot5 * -sin(theta_leftfoot5 + leftankle_q); 1];
156leftfoot6_leftankle_q = [l_leftfoot6 * -cos(theta_leftfoot6 + leftankle_q); l_leftfoot6 * -sin(theta_leftfoot6 + leftankle_q); 1];
157rightfoot1_rightankle_q = [l_rightfoot1 * -cos(theta_rightfoot1 + rightankle_q); l_rightfoot1 * -sin(theta_rightfoot1 + rightankle_q); 1];
158rightfoot2_rightankle_q = [l_rightfoot2 * -cos(theta_rightfoot2 + rightankle_q); l_rightfoot2 * -sin(theta_rightfoot2 + rightankle_q); 1];
159rightfoot3_rightankle_q = [l_rightfoot3 * -cos(theta_rightfoot3 + rightankle_q); l_rightfoot3 * -sin(theta_rightfoot3 + rightankle_q); 1];
160rightfoot4_rightankle_q = [l_rightfoot4 * -cos(theta_rightfoot4 + rightankle_q); l_rightfoot4 * -sin(theta_rightfoot4 + rightankle_q); 1];
161rightfoot5_rightankle_q = [l_rightfoot5 * -cos(theta_rightfoot5 + rightankle_q); l_rightfoot5 * -sin(theta_rightfoot5 + rightankle_q); 1];
162rightfoot6_rightankle_q = [l_rightfoot6 * -cos(theta_rightfoot6 + rightankle_q); l_rightfoot6 * -sin(theta_rightfoot6 + rightankle_q); 1];
163dTj(4:6, 1) = lefthip_origin_x;
164dTj(4:6, 2) = lefthip_origin_y;
165dTj(4:6, 3) = lefthip_origin_q;
166dTj(7:9, 4) = leftknee_lefthip_q;
167dTj(7:9, 1) = lefthip_origin_x;
168dTj(7:9, 2) = lefthip_origin_y;
169dTj(7:9, 3) = lefthip_origin_q;
170dTj(10:12, 5) = leftankle_leftknee_q;
171dTj(10:12, 4) = leftknee_lefthip_q;
172dTj(10:12, 1) = lefthip_origin_x;
173dTj(10:12, 2) = lefthip_origin_y;
174dTj(10:12, 3) = lefthip_origin_q;
175dTj(13:15, 1) = righthip_origin_x;
176dTj(13:15, 2) = righthip_origin_y;
177dTj(13:15, 3) = righthip_origin_q;
178dTj(16:18, 7) = rightknee_righthip_q;
179dTj(16:18, 1) = righthip_origin_x;
180dTj(16:18, 2) = righthip_origin_y;
181dTj(16:18, 3) = righthip_origin_q;
182dTj(19:21, 8) = rightankle_rightknee_q;
183dTj(19:21, 7) = rightknee_righthip_q;
184dTj(19:21, 1) = righthip_origin_x;
185dTj(19:21, 2) = righthip_origin_y;
186dTj(19:21, 3) = righthip_origin_q;
187dTm(1:3, 1) = torso_origin_x;
188dTm(1:3, 2) = torso_origin_y;
189dTm(1:3, 3) = torso_origin_q;
190dTm(4:6, 4) = leftthigh_lefthip_q;
191dTm(4:6, 1) = lefthip_origin_x;
192dTm(4:6, 2) = lefthip_origin_y;
193dTm(4:6, 3) = lefthip_origin_q;
194dTm(7:9, 5) = leftshank_leftknee_q;
195dTm(7:9, 4) = leftknee_lefthip_q;
196dTm(7:9, 1) = lefthip_origin_x;
197dTm(7:9, 2) = lefthip_origin_y;
198dTm(7:9, 3) = lefthip_origin_q;
199dTm(10:12, 6) = leftfoot_leftankle_q;
200dTm(10:12, 5) = leftankle_leftknee_q;
201dTm(10:12, 4) = leftknee_lefthip_q;
202dTm(10:12, 1) = lefthip_origin_x;
203dTm(10:12, 2) = lefthip_origin_y;
204dTm(10:12, 3) = lefthip_origin_q;
205dTm(13:15, 7) = rightthigh_righthip_q;
206dTm(13:15, 1) = righthip_origin_x;
207dTm(13:15, 2) = righthip_origin_y;
208dTm(13:15, 3) = righthip_origin_q;
209dTm(16:18, 8) = rightshank_rightknee_q;
210dTm(16:18, 7) = rightknee_righthip_q;
211dTm(16:18, 1) = righthip_origin_x;
212dTm(16:18, 2) = righthip_origin_y;
213dTm(16:18, 3) = righthip_origin_q;
214dTm(19:21, 9) = rightfoot_rightankle_q;
215dTm(19:21, 8) = rightankle_rightknee_q;
216dTm(19:21, 7) = rightknee_righthip_q;
217dTm(19:21, 1) = righthip_origin_x;
218dTm(19:21, 2) = righthip_origin_y;
219dTm(19:21, 3) = righthip_origin_q;
220dTp(1:3, 6) = leftfoot1_leftankle_q;
221dTp(1:3, 5) = leftankle_leftknee_q;
222dTp(1:3, 4) = leftknee_lefthip_q;
223dTp(1:3, 1) = lefthip_origin_x;
224dTp(1:3, 2) = lefthip_origin_y;
225dTp(1:3, 3) = lefthip_origin_q;
226dTp(4:6, 6) = leftfoot2_leftankle_q;
227dTp(4:6, 5) = leftankle_leftknee_q;
228dTp(4:6, 4) = leftknee_lefthip_q;
229dTp(4:6, 1) = lefthip_origin_x;
230dTp(4:6, 2) = lefthip_origin_y;
231dTp(4:6, 3) = lefthip_origin_q;
232dTp(7:9, 6) = leftfoot3_leftankle_q;
233dTp(7:9, 5) = leftankle_leftknee_q;
234dTp(7:9, 4) = leftknee_lefthip_q;
235dTp(7:9, 1) = lefthip_origin_x;
236dTp(7:9, 2) = lefthip_origin_y;
237dTp(7:9, 3) = lefthip_origin_q;
238dTp(10:12, 6) = leftfoot4_leftankle_q;
239dTp(10:12, 5) = leftankle_leftknee_q;
240dTp(10:12, 4) = leftknee_lefthip_q;
241dTp(10:12, 1) = lefthip_origin_x;
242dTp(10:12, 2) = lefthip_origin_y;
243dTp(10:12, 3) = lefthip_origin_q;
244dTp(13:15, 6) = leftfoot5_leftankle_q;
245dTp(13:15, 5) = leftankle_leftknee_q;
246dTp(13:15, 4) = leftknee_lefthip_q;
247dTp(13:15, 1) = lefthip_origin_x;
248dTp(13:15, 2) = lefthip_origin_y;
249dTp(13:15, 3) = lefthip_origin_q;
250dTp(16:18, 6) = leftfoot6_leftankle_q;
251dTp(16:18, 5) = leftankle_leftknee_q;
252dTp(16:18, 4) = leftknee_lefthip_q;
253dTp(16:18, 1) = lefthip_origin_x;
254dTp(16:18, 2) = lefthip_origin_y;
255dTp(16:18, 3) = lefthip_origin_q;
256dTp(19:21, 9) = rightfoot1_rightankle_q;
257dTp(19:21, 8) = rightankle_rightknee_q;
258dTp(19:21, 7) = rightknee_righthip_q;
259dTp(19:21, 1) = righthip_origin_x;
260dTp(19:21, 2) = righthip_origin_y;
261dTp(19:21, 3) = righthip_origin_q;
262dTp(22:24, 9) = rightfoot2_rightankle_q;
263dTp(22:24, 8) = rightankle_rightknee_q;
264dTp(22:24, 7) = rightknee_righthip_q;
265dTp(22:24, 1) = righthip_origin_x;
266dTp(22:24, 2) = righthip_origin_y;
267dTp(22:24, 3) = righthip_origin_q;
268dTp(25:27, 9) = rightfoot3_rightankle_q;
269dTp(25:27, 8) = rightankle_rightknee_q;
270dTp(25:27, 7) = rightknee_righthip_q;
271dTp(25:27, 1) = righthip_origin_x;
272dTp(25:27, 2) = righthip_origin_y;
273dTp(25:27, 3) = righthip_origin_q;
274dTp(28:30, 9) = rightfoot4_rightankle_q;
275dTp(28:30, 8) = rightankle_rightknee_q;
276dTp(28:30, 7) = rightknee_righthip_q;
277dTp(28:30, 1) = righthip_origin_x;
278dTp(28:30, 2) = righthip_origin_y;
279dTp(28:30, 3) = righthip_origin_q;
280dTp(31:33, 9) = rightfoot5_rightankle_q;
281dTp(31:33, 8) = rightankle_rightknee_q;
282dTp(31:33, 7) = rightknee_righthip_q;
283dTp(31:33, 1) = righthip_origin_x;
284dTp(31:33, 2) = righthip_origin_y;
285dTp(31:33, 3) = righthip_origin_q;
286dTp(34:36, 9) = rightfoot6_rightankle_q;
287dTp(34:36, 8) = rightankle_rightknee_q;
288dTp(34:36, 7) = rightknee_righthip_q;
289dTp(34:36, 1) = righthip_origin_x;
290dTp(34:36, 2) = righthip_origin_y;
291dTp(34:36, 3) = righthip_origin_q;
292lefthip_origin_x = [0; 0; 0];
293lefthip_origin_y = [0; 0; 0];
294lefthip_origin_q = [origin_dx * l_lefthip * sin(theta_lefthip + origin_q); origin_dx * l_lefthip * -cos(theta_lefthip + origin_q); 0];
295leftknee_lefthip_q = [lefthip_dq * l_leftknee * sin(theta_leftknee + lefthip_q); lefthip_dq * l_leftknee * -cos(theta_leftknee + lefthip_q); 0];
296leftankle_leftknee_q = [leftknee_dq * l_leftankle * sin(theta_leftankle + leftknee_q); leftknee_dq * l_leftankle * -cos(theta_leftankle + leftknee_q); 0];
297righthip_origin_x = [0; 0; 0];
298righthip_origin_y = [0; 0; 0];
299righthip_origin_q = [origin_dx * l_righthip * sin(theta_righthip + origin_q); origin_dx * l_righthip * -cos(theta_righthip + origin_q); 0];
300rightknee_righthip_q = [righthip_dq * l_rightknee * sin(theta_rightknee + righthip_q); righthip_dq * l_rightknee * -cos(theta_rightknee + righthip_q); 0];
301rightankle_rightknee_q = [rightknee_dq * l_rightankle * sin(theta_rightankle + rightknee_q); rightknee_dq * l_rightankle * -cos(theta_rightankle + rightknee_q); 0];
302torso_origin_x = [0; 0; 0];
303torso_origin_y = [0; 0; 0];
304torso_origin_q = [origin_dx * l_torso * sin(theta_torso + origin_q); origin_dx * l_torso * -cos(theta_torso + origin_q); 0];
305leftthigh_lefthip_q = [lefthip_dq * l_leftthigh * sin(theta_leftthigh + lefthip_q); lefthip_dq * l_leftthigh * -cos(theta_leftthigh + lefthip_q); 0];
306leftshank_leftknee_q = [leftknee_dq * l_leftshank * sin(theta_leftshank + leftknee_q); leftknee_dq * l_leftshank * -cos(theta_leftshank + leftknee_q); 0];
307leftfoot_leftankle_q = [leftankle_dq * l_leftfoot * sin(theta_leftfoot + leftankle_q); leftankle_dq * l_leftfoot * -cos(theta_leftfoot + leftankle_q); 0];
308rightthigh_righthip_q = [righthip_dq * l_rightthigh * sin(theta_rightthigh + righthip_q); righthip_dq * l_rightthigh * -cos(theta_rightthigh + righthip_q); 0];
309rightshank_rightknee_q = [rightknee_dq * l_rightshank * sin(theta_rightshank + rightknee_q); rightknee_dq * l_rightshank * -cos(theta_rightshank + rightknee_q); 0];
310rightfoot_rightankle_q = [rightankle_dq * l_rightfoot * sin(theta_rightfoot + rightankle_q); rightankle_dq * l_rightfoot * -cos(theta_rightfoot + rightankle_q); 0];
311ddTm(1:3, 1) = torso_origin_x;
312ddTm(1:3, 2) = torso_origin_y;
313ddTm(1:3, 3) = torso_origin_q;
314ddTm(4:6, 4) = leftthigh_lefthip_q;
315ddTm(4:6, 1) = lefthip_origin_x;
316ddTm(4:6, 2) = lefthip_origin_y;
317ddTm(4:6, 3) = lefthip_origin_q;
318ddTm(7:9, 5) = leftshank_leftknee_q;
319ddTm(7:9, 4) = leftknee_lefthip_q;
320ddTm(7:9, 1) = lefthip_origin_x;
321ddTm(7:9, 2) = lefthip_origin_y;
322ddTm(7:9, 3) = lefthip_origin_q;
323ddTm(10:12, 6) = leftfoot_leftankle_q;
324ddTm(10:12, 5) = leftankle_leftknee_q;
325ddTm(10:12, 4) = leftknee_lefthip_q;
326ddTm(10:12, 1) = lefthip_origin_x;
327ddTm(10:12, 2) = lefthip_origin_y;
328ddTm(10:12, 3) = lefthip_origin_q;
329ddTm(13:15, 7) = rightthigh_righthip_q;
330ddTm(13:15, 1) = righthip_origin_x;
331ddTm(13:15, 2) = righthip_origin_y;
332ddTm(13:15, 3) = righthip_origin_q;
333ddTm(16:18, 8) = rightshank_rightknee_q;
334ddTm(16:18, 7) = rightknee_righthip_q;
335ddTm(16:18, 1) = righthip_origin_x;
336ddTm(16:18, 2) = righthip_origin_y;
337ddTm(16:18, 3) = righthip_origin_q;
338ddTm(19:21, 9) = rightfoot_rightankle_q;
339ddTm(19:21, 8) = rightankle_rightknee_q;
340ddTm(19:21, 7) = rightknee_righthip_q;
341ddTm(19:21, 1) = righthip_origin_x;
342ddTm(19:21, 2) = righthip_origin_y;
343ddTm(19:21, 3) = righthip_origin_q;