1function [Tj Tm Tp dTj dTm dTp ddTm M] = Tmatrices(q, dq, p_in)
  2%#codegen 
  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;