-
Notifications
You must be signed in to change notification settings - Fork 0
/
gait2dp_dyn.c
2059 lines (2052 loc) · 106 KB
/
gait2dp_dyn.c
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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// This file was generated by autolevclean.c and contains C code generated by Autolev
#include <math.h>
#include "gait2d.h"
void gait2d_dyn(param_struct* par, double q[NDOF], double qd[NDOF], double qdd[NDOF],
double Vsurface[2], double mom[NMOM], double Zero[NDOF], double dz_dq[NDOF][NDOF],
double dz_dqd[NDOF][NDOF], double dz_dqdd[NDOF][NDOF],
double dz_dmom[NDOF][NMOM], double GRF[4], double dGRF_dq[4][NDOF],
double dGRF_dqd[4][NDOF],
double Stick[NSTICK][2], double tmp[16]) {
// Inputs:
// par.........Struct with model parameters, see gait2dp.h for details.
// q,qd,qdd....Generalized coordinates, velocities, accelerations
// Vsurface....Surface velocity, Left and Right are separate for specificate situations.
// mom.........Moments applied on six joints: right hip/knee/ankle, left hip/knee/ankle
// Outputs:
// Zero...........Residule of dynamics functions after given q, qd, qdd, and mom
// dz_dq, dz_dqd, dz_dqdd..... Jacobians of QQ with respect to q,qd,qdd
// GRF.........Fx,Fy,Mz on right foot, Fx,Fy,Mz on left foot
// dGRF_dq, dGRF_dqd.....Jacobians of GRF with respect to q, qd,
// stick.......X and Y coordinates of 10 points: trunkCM, hip, Rknee, Rankle, Rheel, Rtoe, Lknee, Lankle, Lheel, Ltoe
// tmp.........Velocities and Forces of right/left heel and toe
static double z[1000];
const double gravity = 9.81;
double dRHeel,yRHeel,FxRHeel,FyRHeel,xdotRHeel,ydotRHeel;
double dLHeel,yLHeel,FxLHeel,FyLHeel,xdotLHeel,ydotLHeel;
double dRToe,yRToe,FxRToe,FyRToe,xdotRToe,ydotRToe;
double dLToe,yLToe,FxLToe,FyLToe,xdotLToe,ydotLToe;
double q1 = q[0];
double q1p = qd[0];
double q1pp = qdd[0];
double q2 = q[1];
double q2p = qd[1];
double q2pp = qdd[1];
double q3 = q[2];
double q3p = qd[2];
double q3pp = qdd[2];
double q4 = q[3];
double q4p = qd[3];
double q4pp = qdd[3];
double q5 = q[4];
double q5p = qd[4];
double q5pp = qdd[4];
double q6 = q[5];
double q6p = qd[5];
double q6pp = qdd[5];
double q7 = q[6];
double q7p = qd[6];
double q7pp = qdd[6];
double q8 = q[7];
double q8p = qd[7];
double q8pp = qdd[7];
double q9 = q[8];
double q9p = qd[8];
double q9pp = qdd[8];
double VsurfaceR = Vsurface[0];
double VsurfaceL = Vsurface[1];
double mom1 = mom[0];
double mom2 = mom[1];
double mom3 = mom[2];
double mom4 = mom[3];
double mom5 = mom[4];
double mom6 = mom[5];
z[1] = cos(q3);
z[2] = sin(q3);
z[3] = par->TrunkCMy*q3p;
z[4] = q3p*z[3];
z[5] = cos(q4);
z[6] = sin(q4);
z[7] = z[1]*z[5] - z[2]*z[6];
z[8] = -z[1]*z[6] - z[2]*z[5];
z[9] = z[1]*z[6] + z[2]*z[5];
z[10] = par->ThighCMy*(q3p+q4p);
z[11] = q3p + q4p;
z[12] = z[10]*z[11];
z[13] = cos(q5);
z[14] = sin(q5);
z[15] = par->ThighLen*(q3p+q4p);
z[16] = z[11]*z[15];
z[17] = z[7]*z[13] + z[8]*z[14];
z[18] = z[8]*z[13] - z[7]*z[14];
z[19] = z[7]*z[14] + z[9]*z[13];
z[20] = z[7]*z[13] - z[9]*z[14];
z[21] = par->ThighLen*z[13];
z[22] = par->ThighLen*z[14];
z[23] = z[21] - par->ShankCMy;
z[24] = z[14]*z[16];
z[25] = z[13]*z[16];
z[26] = par->ShankCMy*(q3p+q4p+q5p);
z[27] = q3p + q4p + q5p;
z[28] = z[25] - z[26]*z[27];
z[29] = cos(q6);
z[30] = sin(q6);
z[31] = par->ShankLen + z[21];
z[32] = par->ShankLen*(q3p+q4p+q5p);
z[33] = z[25] + z[27]*z[32];
z[34] = z[17]*z[29] + z[18]*z[30];
z[35] = z[18]*z[29] - z[17]*z[30];
z[36] = z[19]*z[29] + z[20]*z[30];
z[37] = z[20]*z[29] - z[19]*z[30];
z[38] = par->ShankLen*z[29];
z[39] = z[29]*z[31] - z[22]*z[30];
z[40] = -z[22]*z[29] - z[30]*z[31];
z[41] = par->ShankLen*z[30];
z[42] = z[38] - par->FootCMy;
z[43] = z[39] - par->FootCMy;
z[44] = par->FootCMx + z[40];
z[45] = par->FootCMx - z[41];
z[46] = z[29]*z[24] + z[30]*z[33];
z[47] = z[29]*z[33] - z[30]*z[24];
z[48] = par->FootCMx*(q3p+q4p+q5p+q6p);
z[49] = par->FootCMy*(q3p+q4p+q5p+q6p);
z[50] = q3p + q4p + q5p + q6p;
z[51] = z[46] - z[48]*z[50];
z[52] = z[47] - z[49]*z[50];
z[53] = cos(q7);
z[54] = sin(q7);
z[55] = z[1]*z[53] - z[2]*z[54];
z[56] = -z[1]*z[54] - z[2]*z[53];
z[57] = z[1]*z[54] + z[2]*z[53];
z[58] = par->ThighCMy*(q3p+q7p);
z[59] = q3p + q7p;
z[60] = z[58]*z[59];
z[61] = cos(q8);
z[62] = sin(q8);
z[63] = par->ThighLen*(q3p+q7p);
z[64] = z[59]*z[63];
z[65] = z[55]*z[61] + z[56]*z[62];
z[66] = z[56]*z[61] - z[55]*z[62];
z[67] = z[55]*z[62] + z[57]*z[61];
z[68] = z[55]*z[61] - z[57]*z[62];
z[69] = par->ThighLen*z[61];
z[70] = par->ThighLen*z[62];
z[71] = z[69] - par->ShankCMy;
z[72] = z[62]*z[64];
z[73] = z[61]*z[64];
z[74] = par->ShankCMy*(q3p+q7p+q8p);
z[75] = q3p + q7p + q8p;
z[76] = z[73] - z[74]*z[75];
z[77] = cos(q9);
z[78] = sin(q9);
z[79] = par->ShankLen + z[69];
z[80] = par->ShankLen*(q3p+q7p+q8p);
z[81] = z[73] + z[75]*z[80];
z[82] = z[65]*z[77] + z[66]*z[78];
z[83] = z[66]*z[77] - z[65]*z[78];
z[84] = z[67]*z[77] + z[68]*z[78];
z[85] = z[68]*z[77] - z[67]*z[78];
z[86] = par->ShankLen*z[77];
z[87] = z[77]*z[79] - z[70]*z[78];
z[88] = -z[70]*z[77] - z[78]*z[79];
z[89] = par->ShankLen*z[78];
z[90] = z[86] - par->FootCMy;
z[91] = z[87] - par->FootCMy;
z[92] = par->FootCMx + z[88];
z[93] = par->FootCMx - z[89];
z[94] = z[77]*z[72] + z[78]*z[81];
z[95] = z[77]*z[81] - z[78]*z[72];
z[96] = par->FootCMx*(q3p+q7p+q8p+q9p);
z[97] = par->FootCMy*(q3p+q7p+q8p+q9p);
z[98] = q3p + q7p + q8p + q9p;
z[99] = z[94] - z[96]*z[98];
z[100] = z[95] - z[97]*z[98];
z[101] = gravity*par->FootMass;
z[102] = gravity*par->ShankMass;
z[103] = gravity*par->ThighMass;
z[104] = gravity*par->TrunkMass;
z[105] = z[38] - par->ContactY;
z[106] = z[39] - par->ContactY;
z[107] = par->ContactHeelX + z[40];
z[108] = par->ContactHeelX - z[41];
yRHeel = q2 + par->ContactHeelX*z[36] + par->ContactY*z[37] - par->ShankLen*
z[20] - par->ThighLen*z[7];
dRHeel = 0.5*pow((pow(par->ContactY0,2)+pow(yRHeel,2)),0.5) - 0.5*yRHeel;
xdotRHeel = z[35]*(par->ContactHeelX*q6p+z[35]*q1p+z[37]*q2p+z[107]*q3p+
z[107]*q4p+z[108]*q5p) - z[34]*(par->ContactY*q6p-z[34]*q1p-z[36]*q2p-
z[105]*q5p-z[106]*q3p-z[106]*q4p);
ydotRHeel = z[37]*(par->ContactHeelX*q6p+z[35]*q1p+z[37]*q2p+z[107]*q3p+
z[107]*q4p+z[108]*q5p) - z[36]*(par->ContactY*q6p-z[34]*q1p-z[36]*q2p-
z[105]*q5p-z[106]*q3p-z[106]*q4p);
FyRHeel = par->ContactStiff*dRHeel*(1-par->ContactDamp*ydotRHeel) - yRHeel;
FxRHeel = -par->ContactFric*FyRHeel*(xdotRHeel-VsurfaceR)/pow((pow(par->ContactV0,2)+
pow((xdotRHeel-VsurfaceR),2)),0.5);
z[109] = par->ContactToeX + z[40];
z[110] = par->ContactToeX - z[41];
yRToe = q2 + par->ContactToeX*z[36] + par->ContactY*z[37] - par->ShankLen*
z[20] - par->ThighLen*z[7];
dRToe = 0.5*pow((pow(par->ContactY0,2)+pow(yRToe,2)),0.5) - 0.5*yRToe;
xdotRToe = z[35]*(par->ContactToeX*q6p+z[35]*q1p+z[37]*q2p+z[109]*q3p+
z[109]*q4p+z[110]*q5p) - z[34]*(par->ContactY*q6p-z[34]*q1p-z[36]*q2p-
z[105]*q5p-z[106]*q3p-z[106]*q4p);
ydotRToe = z[37]*(par->ContactToeX*q6p+z[35]*q1p+z[37]*q2p+z[109]*q3p+
z[109]*q4p+z[110]*q5p) - z[36]*(par->ContactY*q6p-z[34]*q1p-z[36]*q2p-
z[105]*q5p-z[106]*q3p-z[106]*q4p);
FyRToe = par->ContactStiff*dRToe*(1-par->ContactDamp*ydotRToe) - yRToe;
FxRToe = -par->ContactFric*FyRToe*(xdotRToe-VsurfaceR)/pow((pow(par->ContactV0,2)+pow(
(xdotRToe-VsurfaceR),2)),0.5);
z[111] = z[86] - par->ContactY;
z[112] = z[87] - par->ContactY;
z[113] = par->ContactHeelX + z[88];
z[114] = par->ContactHeelX - z[89];
yLHeel = q2 + par->ContactHeelX*z[84] + par->ContactY*z[85] - par->ShankLen*
z[68] - par->ThighLen*z[55];
dLHeel = 0.5*pow((pow(par->ContactY0,2)+pow(yLHeel,2)),0.5) - 0.5*yLHeel;
xdotLHeel = z[83]*(par->ContactHeelX*q9p+z[83]*q1p+z[85]*q2p+z[113]*q3p+
z[113]*q7p+z[114]*q8p) - z[82]*(par->ContactY*q9p-z[82]*q1p-z[84]*q2p-
z[111]*q8p-z[112]*q3p-z[112]*q7p);
ydotLHeel = z[85]*(par->ContactHeelX*q9p+z[83]*q1p+z[85]*q2p+z[113]*q3p+
z[113]*q7p+z[114]*q8p) - z[84]*(par->ContactY*q9p-z[82]*q1p-z[84]*q2p-
z[111]*q8p-z[112]*q3p-z[112]*q7p);
FyLHeel = par->ContactStiff*dLHeel*(1-par->ContactDamp*ydotLHeel) - yLHeel;
FxLHeel = -par->ContactFric*FyLHeel*(xdotLHeel-VsurfaceL)/pow((pow(par->ContactV0,2)+
pow((xdotLHeel-VsurfaceL),2)),0.5);
z[115] = par->ContactToeX + z[88];
z[116] = par->ContactToeX - z[89];
yLToe = q2 + par->ContactToeX*z[84] + par->ContactY*z[85] - par->ShankLen*
z[68] - par->ThighLen*z[55];
dLToe = 0.5*pow((pow(par->ContactY0,2)+pow(yLToe,2)),0.5) - 0.5*yLToe;
xdotLToe = z[83]*(par->ContactToeX*q9p+z[83]*q1p+z[85]*q2p+z[115]*q3p+
z[115]*q7p+z[116]*q8p) - z[82]*(par->ContactY*q9p-z[82]*q1p-z[84]*q2p-
z[111]*q8p-z[112]*q3p-z[112]*q7p);
ydotLToe = z[85]*(par->ContactToeX*q9p+z[83]*q1p+z[85]*q2p+z[115]*q3p+
z[115]*q7p+z[116]*q8p) - z[84]*(par->ContactY*q9p-z[82]*q1p-z[84]*q2p-
z[111]*q8p-z[112]*q3p-z[112]*q7p);
FyLToe = par->ContactStiff*dLToe*(1-par->ContactDamp*ydotLToe) - yLToe;
FxLToe = -par->ContactFric*FyLToe*(xdotLToe-VsurfaceL)/pow((pow(par->ContactV0,2)+pow(
(xdotLToe-VsurfaceL),2)),0.5);
z[117] = -1 - 0.5*par->ContactStiff*(1-yRHeel/pow((pow(par->ContactY0,2)+
pow(yRHeel,2)),0.5))*(1-par->ContactDamp*ydotRHeel);
z[118] = par->ContactFric*(xdotRHeel-VsurfaceR)*z[117]/pow((pow(par->ContactV0,2)+pow(
(xdotRHeel-VsurfaceR),2)),0.5);
z[119] = -1 - 0.5*par->ContactStiff*(1-yRToe/pow((pow(par->ContactY0,2)+
pow(yRToe,2)),0.5))*(1-par->ContactDamp*ydotRToe);
z[120] = par->ContactFric*(xdotRToe-VsurfaceR)*z[119]/pow((pow(par->ContactV0,2)+pow(
(xdotRToe-VsurfaceR),2)),0.5);
z[121] = par->ContactHeelX*z[34] + par->ContactY*z[35] - par->ShankLen*
z[18] - par->ThighLen*z[8];
z[122] = z[121]*(1-yRHeel/pow((pow(par->ContactY0,2)+pow(yRHeel,2)),0.5));
z[123] = z[2]*z[6] - z[1]*z[5];
z[124] = z[13]*z[123] - z[8]*z[14];
z[125] = z[8]*z[13] + z[14]*z[123];
z[126] = z[29]*z[124] - z[30]*z[125];
z[127] = z[29]*z[125] + z[30]*z[124];
z[128] = z[36]*(z[127]*q1p+2*z[34]*q2p) + z[37]*(z[126]*q1p+2*z[35]*q2p) +
z[35]*(par->ContactHeelX*q6p+z[35]*q1p+z[107]*q3p+z[107]*q4p+z[108]*q5p) -
z[34]*(par->ContactY*q6p-z[34]*q1p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[129] = -z[121] - par->ContactDamp*par->ContactStiff*dRHeel*z[128] - 0.5*
par->ContactStiff*z[122]*(1-par->ContactDamp*ydotRHeel);
z[130] = z[34]*(z[34]*q2p+2*z[127]*q1p) + z[35]*(z[35]*q2p+2*z[126]*q1p) +
z[126]*(par->ContactHeelX*q6p+z[37]*q2p+z[107]*q3p+z[107]*q4p+z[108]*q5p) -
z[127]*(par->ContactY*q6p-z[36]*q2p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[131] = par->ContactFric*(pow(par->ContactV0,2)*FyRHeel*z[130]+(xdotRHeel-VsurfaceR)*
z[129]*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRHeel-VsurfaceR),2)),1.5);
z[132] = par->ContactToeX*z[34] + par->ContactY*z[35] - par->ShankLen*z[18] -
par->ThighLen*z[8];
z[133] = z[132]*(1-yRToe/pow((pow(par->ContactY0,2)+pow(yRToe,2)),0.5));
z[134] = z[36]*(z[127]*q1p+2*z[34]*q2p) + z[37]*(z[126]*q1p+2*z[35]*q2p) +
z[35]*(par->ContactToeX*q6p+z[35]*q1p+z[109]*q3p+z[109]*q4p+z[110]*q5p) -
z[34]*(par->ContactY*q6p-z[34]*q1p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[135] = -z[132] - par->ContactDamp*par->ContactStiff*dRToe*z[134] - 0.5*
par->ContactStiff*z[133]*(1-par->ContactDamp*ydotRToe);
z[136] = z[34]*(z[34]*q2p+2*z[127]*q1p) + z[35]*(z[35]*q2p+2*z[126]*q1p) +
z[126]*(par->ContactToeX*q6p+z[37]*q2p+z[109]*q3p+z[109]*q4p+z[110]*q5p) -
z[127]*(par->ContactY*q6p-z[36]*q2p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[137] = par->ContactFric*(pow(par->ContactV0,2)*FyRToe*z[136]+(xdotRToe-VsurfaceR)*
z[135]*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[138] = -z[7]*z[14] - z[9]*z[13];
z[139] = z[20]*z[29] + z[30]*z[138];
z[140] = z[29]*z[138] - z[20]*z[30];
z[141] = par->ContactHeelX*z[139] + par->ContactY*z[140] - par->ShankLen*
z[138];
z[142] = z[141]*(1-yRHeel/pow((pow(par->ContactY0,2)+pow(yRHeel,2)),0.5));
z[143] = -z[7]*z[13] - z[8]*z[14];
z[144] = z[29]*z[143] - z[18]*z[30];
z[145] = par->ThighLen*(z[13]*z[29]-z[14]*z[30]);
z[146] = z[18]*z[29] + z[30]*z[143];
z[147] = par->ThighLen*(z[13]*z[30]+z[14]*z[29]);
z[148] = z[140]*(par->ContactHeelX*q6p+z[35]*q1p+z[107]*q3p+z[107]*q4p+
z[108]*q5p) + z[36]*(z[146]*q1p+2*z[139]*q2p-z[147]*q3p-z[147]*q4p) + z[37]*(
z[144]*q1p+2*z[140]*q2p-z[145]*q3p-z[145]*q4p) - z[139]*(par->ContactY*q6p-
z[34]*q1p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[149] = -z[141] - par->ContactDamp*par->ContactStiff*dRHeel*z[148] - 0.5*
par->ContactStiff*z[142]*(1-par->ContactDamp*ydotRHeel);
z[150] = z[144]*(par->ContactHeelX*q6p+z[37]*q2p+z[107]*q3p+z[107]*q4p+
z[108]*q5p) + z[34]*(z[139]*q2p+2*z[146]*q1p-z[147]*q3p-z[147]*q4p) + z[35]*(
z[140]*q2p+2*z[144]*q1p-z[145]*q3p-z[145]*q4p) - z[146]*(par->ContactY*q6p-
z[36]*q2p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[151] = par->ContactFric*(pow(par->ContactV0,2)*FyRHeel*z[150]+(xdotRHeel-VsurfaceR)*
z[149]*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRHeel-VsurfaceR),2)),1.5);
z[152] = par->ContactToeX*z[139] + par->ContactY*z[140] - par->ShankLen*
z[138];
z[153] = z[152]*(1-yRToe/pow((pow(par->ContactY0,2)+pow(yRToe,2)),0.5));
z[154] = z[140]*(par->ContactToeX*q6p+z[35]*q1p+z[109]*q3p+z[109]*q4p+
z[110]*q5p) + z[36]*(z[146]*q1p+2*z[139]*q2p-z[147]*q3p-z[147]*q4p) + z[37]*(
z[144]*q1p+2*z[140]*q2p-z[145]*q3p-z[145]*q4p) - z[139]*(par->ContactY*q6p-
z[34]*q1p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[155] = -z[152] - par->ContactDamp*par->ContactStiff*dRToe*z[154] - 0.5*
par->ContactStiff*z[153]*(1-par->ContactDamp*ydotRToe);
z[156] = z[144]*(par->ContactToeX*q6p+z[37]*q2p+z[109]*q3p+z[109]*q4p+
z[110]*q5p) + z[34]*(z[139]*q2p+2*z[146]*q1p-z[147]*q3p-z[147]*q4p) + z[35]*(
z[140]*q2p+2*z[144]*q1p-z[145]*q3p-z[145]*q4p) - z[146]*(par->ContactY*q6p-
z[36]*q2p-z[105]*q5p-z[106]*q3p-z[106]*q4p);
z[157] = par->ContactFric*(pow(par->ContactV0,2)*FyRToe*z[156]+(xdotRToe-VsurfaceR)*
z[155]*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[158] = -z[19]*z[29] - z[20]*z[30];
z[159] = par->ContactHeelX*z[37] + par->ContactY*z[158];
z[160] = z[159]*(1-yRHeel/pow((pow(par->ContactY0,2)+pow(yRHeel,2)),0.5));
z[161] = -z[17]*z[29] - z[18]*z[30];
z[162] = z[22]*z[30] - z[29]*z[31];
z[163] = z[37]*(z[161]*q1p+z[162]*q3p+z[162]*q4p-par->ShankLen*z[29]*q5p) +
z[36]*(z[35]*q1p+z[40]*q3p+z[40]*q4p+2*z[37]*q2p-par->ShankLen*z[30]*q5p) +
z[158]*(par->ContactHeelX*q6p+z[35]*q1p+z[107]*q3p+z[107]*q4p+z[108]*q5p+2*
z[37]*q2p) - z[37]*(par->ContactY*q6p-z[34]*q1p-z[105]*q5p-z[106]*q3p-
z[106]*q4p);
z[164] = -z[159] - par->ContactDamp*par->ContactStiff*dRHeel*z[163] - 0.5*
par->ContactStiff*z[160]*(1-par->ContactDamp*ydotRHeel);
z[165] = z[35]*(z[158]*q2p+z[162]*q3p+z[162]*q4p-par->ShankLen*z[29]*q5p) +
z[34]*(z[37]*q2p+z[40]*q3p+z[40]*q4p+2*z[35]*q1p-par->ShankLen*z[30]*q5p) +
z[161]*(par->ContactHeelX*q6p+z[37]*q2p+z[107]*q3p+z[107]*q4p+z[108]*q5p+2*
z[35]*q1p) - z[35]*(par->ContactY*q6p-z[36]*q2p-z[105]*q5p-z[106]*q3p-
z[106]*q4p);
z[166] = par->ContactFric*(pow(par->ContactV0,2)*FyRHeel*z[165]+(xdotRHeel-VsurfaceR)*
z[164]*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRHeel-VsurfaceR),2)),1.5);
z[167] = par->ContactToeX*z[37] + par->ContactY*z[158];
z[168] = z[167]*(1-yRToe/pow((pow(par->ContactY0,2)+pow(yRToe,2)),0.5));
z[169] = z[37]*(z[161]*q1p+z[162]*q3p+z[162]*q4p-par->ShankLen*z[29]*q5p) +
z[36]*(z[35]*q1p+z[40]*q3p+z[40]*q4p+2*z[37]*q2p-par->ShankLen*z[30]*q5p) +
z[158]*(par->ContactToeX*q6p+z[35]*q1p+z[109]*q3p+z[109]*q4p+z[110]*q5p+2*
z[37]*q2p) - z[37]*(par->ContactY*q6p-z[34]*q1p-z[105]*q5p-z[106]*q3p-
z[106]*q4p);
z[170] = -z[167] - par->ContactDamp*par->ContactStiff*dRToe*z[169] - 0.5*
par->ContactStiff*z[168]*(1-par->ContactDamp*ydotRToe);
z[171] = z[35]*(z[158]*q2p+z[162]*q3p+z[162]*q4p-par->ShankLen*z[29]*q5p) +
z[34]*(z[37]*q2p+z[40]*q3p+z[40]*q4p+2*z[35]*q1p-par->ShankLen*z[30]*q5p) +
z[161]*(par->ContactToeX*q6p+z[37]*q2p+z[109]*q3p+z[109]*q4p+z[110]*q5p+2*
z[35]*q1p) - z[35]*(par->ContactY*q6p-z[36]*q2p-z[105]*q5p-z[106]*q3p-
z[106]*q4p);
z[172] = par->ContactFric*(pow(par->ContactV0,2)*FyRToe*z[171]+(xdotRToe-VsurfaceR)*
z[170]*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[173] = -1 - 0.5*par->ContactStiff*(1-yLHeel/pow((pow(par->ContactY0,2)+
pow(yLHeel,2)),0.5))*(1-par->ContactDamp*ydotLHeel);
z[174] = par->ContactFric*(xdotLHeel-VsurfaceL)*z[173]/pow((pow(par->ContactV0,2)+pow(
(xdotLHeel-VsurfaceL),2)),0.5);
z[175] = -1 - 0.5*par->ContactStiff*(1-yLToe/pow((pow(par->ContactY0,2)+
pow(yLToe,2)),0.5))*(1-par->ContactDamp*ydotLToe);
z[176] = par->ContactFric*(xdotLToe-VsurfaceL)*z[175]/pow((pow(par->ContactV0,2)+pow(
(xdotLToe-VsurfaceL),2)),0.5);
z[177] = par->ContactHeelX*z[82] + par->ContactY*z[83] - par->ShankLen*
z[66] - par->ThighLen*z[56];
z[178] = z[177]*(1-yLHeel/pow((pow(par->ContactY0,2)+pow(yLHeel,2)),0.5));
z[179] = z[2]*z[54] - z[1]*z[53];
z[180] = z[61]*z[179] - z[56]*z[62];
z[181] = z[56]*z[61] + z[62]*z[179];
z[182] = z[77]*z[180] - z[78]*z[181];
z[183] = z[77]*z[181] + z[78]*z[180];
z[184] = z[84]*(z[183]*q1p+2*z[82]*q2p) + z[85]*(z[182]*q1p+2*z[83]*q2p) +
z[83]*(par->ContactHeelX*q9p+z[83]*q1p+z[113]*q3p+z[113]*q7p+z[114]*q8p) -
z[82]*(par->ContactY*q9p-z[82]*q1p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[185] = -z[177] - par->ContactDamp*par->ContactStiff*dLHeel*z[184] - 0.5*
par->ContactStiff*z[178]*(1-par->ContactDamp*ydotLHeel);
z[186] = z[82]*(z[82]*q2p+2*z[183]*q1p) + z[83]*(z[83]*q2p+2*z[182]*q1p) +
z[182]*(par->ContactHeelX*q9p+z[85]*q2p+z[113]*q3p+z[113]*q7p+z[114]*q8p) -
z[183]*(par->ContactY*q9p-z[84]*q2p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[187] = par->ContactFric*(pow(par->ContactV0,2)*FyLHeel*z[186]+(xdotLHeel-VsurfaceL)*
z[185]*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLHeel-VsurfaceL),2)),1.5);
z[188] = par->ContactToeX*z[82] + par->ContactY*z[83] - par->ShankLen*z[66] -
par->ThighLen*z[56];
z[189] = z[188]*(1-yLToe/pow((pow(par->ContactY0,2)+pow(yLToe,2)),0.5));
z[190] = z[84]*(z[183]*q1p+2*z[82]*q2p) + z[85]*(z[182]*q1p+2*z[83]*q2p) +
z[83]*(par->ContactToeX*q9p+z[83]*q1p+z[115]*q3p+z[115]*q7p+z[116]*q8p) -
z[82]*(par->ContactY*q9p-z[82]*q1p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[191] = -z[188] - par->ContactDamp*par->ContactStiff*dLToe*z[190] - 0.5*
par->ContactStiff*z[189]*(1-par->ContactDamp*ydotLToe);
z[192] = z[82]*(z[82]*q2p+2*z[183]*q1p) + z[83]*(z[83]*q2p+2*z[182]*q1p) +
z[182]*(par->ContactToeX*q9p+z[85]*q2p+z[115]*q3p+z[115]*q7p+z[116]*q8p) -
z[183]*(par->ContactY*q9p-z[84]*q2p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[193] = par->ContactFric*(pow(par->ContactV0,2)*FyLToe*z[192]+(xdotLToe-VsurfaceL)*
z[191]*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[194] = -z[55]*z[62] - z[57]*z[61];
z[195] = z[68]*z[77] + z[78]*z[194];
z[196] = z[77]*z[194] - z[68]*z[78];
z[197] = par->ContactHeelX*z[195] + par->ContactY*z[196] - par->ShankLen*
z[194];
z[198] = z[197]*(1-yLHeel/pow((pow(par->ContactY0,2)+pow(yLHeel,2)),0.5));
z[199] = -z[55]*z[61] - z[56]*z[62];
z[200] = z[77]*z[199] - z[66]*z[78];
z[201] = par->ThighLen*(z[61]*z[77]-z[62]*z[78]);
z[202] = z[66]*z[77] + z[78]*z[199];
z[203] = par->ThighLen*(z[61]*z[78]+z[62]*z[77]);
z[204] = z[196]*(par->ContactHeelX*q9p+z[83]*q1p+z[113]*q3p+z[113]*q7p+
z[114]*q8p) + z[84]*(z[202]*q1p+2*z[195]*q2p-z[203]*q3p-z[203]*q7p) + z[85]*(
z[200]*q1p+2*z[196]*q2p-z[201]*q3p-z[201]*q7p) - z[195]*(par->ContactY*q9p-
z[82]*q1p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[205] = -z[197] - par->ContactDamp*par->ContactStiff*dLHeel*z[204] - 0.5*
par->ContactStiff*z[198]*(1-par->ContactDamp*ydotLHeel);
z[206] = z[200]*(par->ContactHeelX*q9p+z[85]*q2p+z[113]*q3p+z[113]*q7p+
z[114]*q8p) + z[82]*(z[195]*q2p+2*z[202]*q1p-z[203]*q3p-z[203]*q7p) + z[83]*(
z[196]*q2p+2*z[200]*q1p-z[201]*q3p-z[201]*q7p) - z[202]*(par->ContactY*q9p-
z[84]*q2p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[207] = par->ContactFric*(pow(par->ContactV0,2)*FyLHeel*z[206]+(xdotLHeel-VsurfaceL)*
z[205]*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLHeel-VsurfaceL),2)),1.5);
z[208] = par->ContactToeX*z[195] + par->ContactY*z[196] - par->ShankLen*
z[194];
z[209] = z[208]*(1-yLToe/pow((pow(par->ContactY0,2)+pow(yLToe,2)),0.5));
z[210] = z[196]*(par->ContactToeX*q9p+z[83]*q1p+z[115]*q3p+z[115]*q7p+
z[116]*q8p) + z[84]*(z[202]*q1p+2*z[195]*q2p-z[203]*q3p-z[203]*q7p) + z[85]*(
z[200]*q1p+2*z[196]*q2p-z[201]*q3p-z[201]*q7p) - z[195]*(par->ContactY*q9p-
z[82]*q1p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[211] = -z[208] - par->ContactDamp*par->ContactStiff*dLToe*z[210] - 0.5*
par->ContactStiff*z[209]*(1-par->ContactDamp*ydotLToe);
z[212] = z[200]*(par->ContactToeX*q9p+z[85]*q2p+z[115]*q3p+z[115]*q7p+
z[116]*q8p) + z[82]*(z[195]*q2p+2*z[202]*q1p-z[203]*q3p-z[203]*q7p) + z[83]*(
z[196]*q2p+2*z[200]*q1p-z[201]*q3p-z[201]*q7p) - z[202]*(par->ContactY*q9p-
z[84]*q2p-z[111]*q8p-z[112]*q3p-z[112]*q7p);
z[213] = par->ContactFric*(pow(par->ContactV0,2)*FyLToe*z[212]+(xdotLToe-VsurfaceL)*
z[211]*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[214] = -z[67]*z[77] - z[68]*z[78];
z[215] = par->ContactHeelX*z[85] + par->ContactY*z[214];
z[216] = z[215]*(1-yLHeel/pow((pow(par->ContactY0,2)+pow(yLHeel,2)),0.5));
z[217] = -z[65]*z[77] - z[66]*z[78];
z[218] = z[70]*z[78] - z[77]*z[79];
z[219] = z[85]*(z[217]*q1p+z[218]*q3p+z[218]*q7p-par->ShankLen*z[77]*q8p) +
z[84]*(z[83]*q1p+z[88]*q3p+z[88]*q7p+2*z[85]*q2p-par->ShankLen*z[78]*q8p) +
z[214]*(par->ContactHeelX*q9p+z[83]*q1p+z[113]*q3p+z[113]*q7p+z[114]*q8p+2*
z[85]*q2p) - z[85]*(par->ContactY*q9p-z[82]*q1p-z[111]*q8p-z[112]*q3p-
z[112]*q7p);
z[220] = -z[215] - par->ContactDamp*par->ContactStiff*dLHeel*z[219] - 0.5*
par->ContactStiff*z[216]*(1-par->ContactDamp*ydotLHeel);
z[221] = z[83]*(z[214]*q2p+z[218]*q3p+z[218]*q7p-par->ShankLen*z[77]*q8p) +
z[82]*(z[85]*q2p+z[88]*q3p+z[88]*q7p+2*z[83]*q1p-par->ShankLen*z[78]*q8p) +
z[217]*(par->ContactHeelX*q9p+z[85]*q2p+z[113]*q3p+z[113]*q7p+z[114]*q8p+2*
z[83]*q1p) - z[83]*(par->ContactY*q9p-z[84]*q2p-z[111]*q8p-z[112]*q3p-
z[112]*q7p);
z[222] = par->ContactFric*(pow(par->ContactV0,2)*FyLHeel*z[221]+(xdotLHeel-VsurfaceL)*
z[220]*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLHeel-VsurfaceL),2)),1.5);
z[223] = par->ContactToeX*z[85] + par->ContactY*z[214];
z[224] = z[223]*(1-yLToe/pow((pow(par->ContactY0,2)+pow(yLToe,2)),0.5));
z[225] = z[85]*(z[217]*q1p+z[218]*q3p+z[218]*q7p-par->ShankLen*z[77]*q8p) +
z[84]*(z[83]*q1p+z[88]*q3p+z[88]*q7p+2*z[85]*q2p-par->ShankLen*z[78]*q8p) +
z[214]*(par->ContactToeX*q9p+z[83]*q1p+z[115]*q3p+z[115]*q7p+z[116]*q8p+2*
z[85]*q2p) - z[85]*(par->ContactY*q9p-z[82]*q1p-z[111]*q8p-z[112]*q3p-
z[112]*q7p);
z[226] = -z[223] - par->ContactDamp*par->ContactStiff*dLToe*z[225] - 0.5*
par->ContactStiff*z[224]*(1-par->ContactDamp*ydotLToe);
z[227] = z[83]*(z[214]*q2p+z[218]*q3p+z[218]*q7p-par->ShankLen*z[77]*q8p) +
z[82]*(z[85]*q2p+z[88]*q3p+z[88]*q7p+2*z[83]*q1p-par->ShankLen*z[78]*q8p) +
z[217]*(par->ContactToeX*q9p+z[85]*q2p+z[115]*q3p+z[115]*q7p+z[116]*q8p+2*
z[83]*q1p) - z[83]*(par->ContactY*q9p-z[84]*q2p-z[111]*q8p-z[112]*q3p-
z[112]*q7p);
z[228] = par->ContactFric*(pow(par->ContactV0,2)*FyLToe*z[227]+(xdotLToe-VsurfaceL)*
z[226]*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[229] = -z[118] - z[120];
z[230] = -z[131] - z[137];
z[231] = -z[151] - z[157];
z[232] = -z[166] - z[172];
z[233] = z[117] + z[119];
z[234] = z[129] + z[135];
z[235] = z[149] + z[155];
z[236] = z[164] + z[170];
z[237] = -z[174] - z[176];
z[238] = -z[187] - z[193];
z[239] = -z[207] - z[213];
z[240] = -z[222] - z[228];
z[241] = z[173] + z[175];
z[242] = z[185] + z[191];
z[243] = z[205] + z[211];
z[244] = z[220] + z[226];
z[245] = z[34]*z[36] + z[35]*z[37];
z[246] = par->ContactDamp*par->ContactStiff*dRHeel*z[245];
z[247] = par->ContactFric*(z[246]*(xdotRHeel-VsurfaceR)*(pow(par->ContactV0,2)+pow(
(xdotRHeel-VsurfaceR),2))-pow(par->ContactV0,2)*(pow(z[34],2)+pow(z[35],2))*FyRHeel)/
pow((pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)),1.5);
z[248] = par->ContactDamp*par->ContactStiff*dRToe*z[245];
z[249] = par->ContactFric*(z[248]*(xdotRToe-VsurfaceR)*(pow(par->ContactV0,2)+pow(
(xdotRToe-VsurfaceR),2))-pow(par->ContactV0,2)*(pow(z[34],2)+pow(z[35],2))*FyRToe)/pow((
pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)),1.5);
z[250] = par->ContactDamp*par->ContactStiff*dRHeel*(pow(z[36],2)+pow(z[37],
2));
z[251] = par->ContactFric*(pow(par->ContactV0,2)*z[245]*FyRHeel-z[250]*
(xdotRHeel-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,
2)+pow((xdotRHeel-VsurfaceR),2)),1.5);
z[252] = par->ContactDamp*par->ContactStiff*dRToe*(pow(z[36],2)+pow(z[37],2));
z[253] = par->ContactFric*(pow(par->ContactV0,2)*z[245]*FyRToe-z[252]*
(xdotRToe-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[254] = z[36]*z[106] + z[37]*z[107];
z[255] = par->ContactDamp*par->ContactStiff*dRHeel*z[254];
z[256] = z[34]*z[106] + z[35]*z[107];
z[257] = par->ContactFric*(pow(par->ContactV0,2)*z[256]*FyRHeel-z[255]*
(xdotRHeel-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,
2)+pow((xdotRHeel-VsurfaceR),2)),1.5);
z[258] = z[36]*z[106] + z[37]*z[109];
z[259] = par->ContactDamp*par->ContactStiff*dRToe*z[258];
z[260] = z[34]*z[106] + z[35]*z[109];
z[261] = par->ContactFric*(pow(par->ContactV0,2)*z[260]*FyRToe-z[259]*
(xdotRToe-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[262] = z[36]*z[105] + z[37]*z[108];
z[263] = par->ContactDamp*par->ContactStiff*dRHeel*z[262];
z[264] = z[34]*z[105] + z[35]*z[108];
z[265] = par->ContactFric*(pow(par->ContactV0,2)*z[264]*FyRHeel-z[263]*
(xdotRHeel-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,
2)+pow((xdotRHeel-VsurfaceR),2)),1.5);
z[266] = z[36]*z[105] + z[37]*z[110];
z[267] = par->ContactDamp*par->ContactStiff*dRToe*z[266];
z[268] = z[34]*z[105] + z[35]*z[110];
z[269] = par->ContactFric*(pow(par->ContactV0,2)*z[268]*FyRToe-z[267]*
(xdotRToe-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[270] = par->ContactHeelX*z[37] - par->ContactY*z[36];
z[271] = par->ContactDamp*par->ContactStiff*dRHeel*z[270];
z[272] = par->ContactHeelX*z[35] - par->ContactY*z[34];
z[273] = par->ContactFric*(pow(par->ContactV0,2)*z[272]*FyRHeel-z[271]*
(xdotRHeel-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRHeel-VsurfaceR),2)))/pow((pow(par->ContactV0,
2)+pow((xdotRHeel-VsurfaceR),2)),1.5);
z[274] = par->ContactToeX*z[37] - par->ContactY*z[36];
z[275] = par->ContactDamp*par->ContactStiff*dRToe*z[274];
z[276] = par->ContactToeX*z[35] - par->ContactY*z[34];
z[277] = par->ContactFric*(pow(par->ContactV0,2)*z[276]*FyRToe-z[275]*
(xdotRToe-VsurfaceR)*(pow(par->ContactV0,2)+pow((xdotRToe-VsurfaceR),2)))/pow((pow(par->ContactV0,2)+
pow((xdotRToe-VsurfaceR),2)),1.5);
z[278] = z[82]*z[84] + z[83]*z[85];
z[279] = par->ContactDamp*par->ContactStiff*dLHeel*z[278];
z[280] = par->ContactFric*(z[279]*(xdotLHeel-VsurfaceL)*(pow(par->ContactV0,2)+pow(
(xdotLHeel-VsurfaceL),2))-pow(par->ContactV0,2)*(pow(z[82],2)+pow(z[83],2))*FyLHeel)/
pow((pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)),1.5);
z[281] = par->ContactDamp*par->ContactStiff*dLToe*z[278];
z[282] = par->ContactFric*(z[281]*(xdotLToe-VsurfaceL)*(pow(par->ContactV0,2)+pow(
(xdotLToe-VsurfaceL),2))-pow(par->ContactV0,2)*(pow(z[82],2)+pow(z[83],2))*FyLToe)/pow((
pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)),1.5);
z[283] = par->ContactDamp*par->ContactStiff*dLHeel*(pow(z[84],2)+pow(z[85],
2));
z[284] = par->ContactFric*(pow(par->ContactV0,2)*z[278]*FyLHeel-z[283]*
(xdotLHeel-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,
2)+pow((xdotLHeel-VsurfaceL),2)),1.5);
z[285] = par->ContactDamp*par->ContactStiff*dLToe*(pow(z[84],2)+pow(z[85],2));
z[286] = par->ContactFric*(pow(par->ContactV0,2)*z[278]*FyLToe-z[285]*
(xdotLToe-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[287] = z[84]*z[112] + z[85]*z[113];
z[288] = par->ContactDamp*par->ContactStiff*dLHeel*z[287];
z[289] = z[82]*z[112] + z[83]*z[113];
z[290] = par->ContactFric*(pow(par->ContactV0,2)*z[289]*FyLHeel-z[288]*
(xdotLHeel-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,
2)+pow((xdotLHeel-VsurfaceL),2)),1.5);
z[291] = z[84]*z[112] + z[85]*z[115];
z[292] = par->ContactDamp*par->ContactStiff*dLToe*z[291];
z[293] = z[82]*z[112] + z[83]*z[115];
z[294] = par->ContactFric*(pow(par->ContactV0,2)*z[293]*FyLToe-z[292]*
(xdotLToe-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[295] = z[84]*z[111] + z[85]*z[114];
z[296] = par->ContactDamp*par->ContactStiff*dLHeel*z[295];
z[297] = z[82]*z[111] + z[83]*z[114];
z[298] = par->ContactFric*(pow(par->ContactV0,2)*z[297]*FyLHeel-z[296]*
(xdotLHeel-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,
2)+pow((xdotLHeel-VsurfaceL),2)),1.5);
z[299] = z[84]*z[111] + z[85]*z[116];
z[300] = par->ContactDamp*par->ContactStiff*dLToe*z[299];
z[301] = z[82]*z[111] + z[83]*z[116];
z[302] = par->ContactFric*(pow(par->ContactV0,2)*z[301]*FyLToe-z[300]*
(xdotLToe-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[303] = par->ContactHeelX*z[85] - par->ContactY*z[84];
z[304] = par->ContactDamp*par->ContactStiff*dLHeel*z[303];
z[305] = par->ContactHeelX*z[83] - par->ContactY*z[82];
z[306] = par->ContactFric*(pow(par->ContactV0,2)*z[305]*FyLHeel-z[304]*
(xdotLHeel-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLHeel-VsurfaceL),2)))/pow((pow(par->ContactV0,
2)+pow((xdotLHeel-VsurfaceL),2)),1.5);
z[307] = par->ContactToeX*z[85] - par->ContactY*z[84];
z[308] = par->ContactDamp*par->ContactStiff*dLToe*z[307];
z[309] = par->ContactToeX*z[83] - par->ContactY*z[82];
z[310] = par->ContactFric*(pow(par->ContactV0,2)*z[309]*FyLToe-z[308]*
(xdotLToe-VsurfaceL)*(pow(par->ContactV0,2)+pow((xdotLToe-VsurfaceL),2)))/pow((pow(par->ContactV0,2)+
pow((xdotLToe-VsurfaceL),2)),1.5);
z[311] = z[247] + z[249];
z[312] = -z[251] - z[253];
z[313] = -z[257] - z[261];
z[314] = -z[265] - z[269];
z[315] = -z[273] - z[277];
z[316] = -z[246] - z[248];
z[317] = -z[250] - z[252];
z[318] = -z[255] - z[259];
z[319] = -z[263] - z[267];
z[320] = -z[271] - z[275];
z[321] = z[280] + z[282];
z[322] = -z[284] - z[286];
z[323] = -z[290] - z[294];
z[324] = -z[298] - z[302];
z[325] = -z[306] - z[310];
z[326] = -z[279] - z[281];
z[327] = -z[283] - z[285];
z[328] = -z[288] - z[292];
z[329] = -z[296] - z[300];
z[330] = -z[304] - z[308];
z[331] = mom1 - mom2;
z[332] = mom2 - mom3;
z[333] = -mom1 - mom4;
z[334] = mom4 - mom5;
z[335] = mom5 - mom6;
z[336] = z[34]*z[36]*FyRHeel + z[34]*z[36]*FyRToe + pow(z[34],2)*FxRHeel +
pow(z[34],2)*FxRToe + z[35]*z[37]*FyRHeel + z[35]*z[37]*FyRToe + pow(z[35],
2)*FxRHeel + pow(z[35],2)*FxRToe + z[82]*z[84]*FyLHeel + z[82]*z[84]*FyLToe +
pow(z[82],2)*FxLHeel + pow(z[82],2)*FxLToe + z[83]*z[85]*FyLHeel + z[83]*
z[85]*FyLToe + pow(z[83],2)*FxLHeel + pow(z[83],2)*FxLToe - z[7]*z[103]*(
z[8]+z[9]) - z[55]*z[103]*(z[56]+z[57]) - z[101]*(z[34]*z[36]+z[35]*z[37]) -
z[101]*(z[82]*z[84]+z[83]*z[85]) - z[102]*(z[17]*z[19]+z[18]*z[20]) -
z[102]*(z[65]*z[67]+z[66]*z[68]);
z[337] = z[34]*z[36]*FxRHeel + z[34]*z[36]*FxRToe + z[35]*z[37]*FxRHeel +
z[35]*z[37]*FxRToe + pow(z[36],2)*FyRHeel + pow(z[36],2)*FyRToe + pow(z[37],
2)*FyRHeel + pow(z[37],2)*FyRToe + z[82]*z[84]*FxLHeel + z[82]*z[84]*FxLToe +
z[83]*z[85]*FxLHeel + z[83]*z[85]*FxLToe + pow(z[84],2)*FyLHeel + pow(z[84],
2)*FyLToe + pow(z[85],2)*FyLHeel + pow(z[85],2)*FyLToe - z[101]*(pow(z[36],
2)+pow(z[37],2)) - z[101]*(pow(z[84],2)+pow(z[85],2)) - z[102]*(pow(z[19],2)+
pow(z[20],2)) - z[102]*(pow(z[67],2)+pow(z[68],2)) - z[103]*(pow(z[7],2)+
pow(z[9],2)) - z[103]*(pow(z[55],2)+pow(z[57],2)) - z[104]*(pow(z[1],2)+
pow(z[2],2));
z[338] = mom3 + mom6 + z[331] + z[332] + z[333] + z[334] + z[335] + par->ThighCMy*
z[9]*z[103] + par->ThighCMy*z[57]*z[103] + par->TrunkCMy*z[2]*z[104] +
z[34]*z[106]*FxRHeel + z[34]*z[106]*FxRToe + z[35]*z[107]*FxRHeel + z[35]*
z[109]*FxRToe + z[36]*z[106]*FyRHeel + z[36]*z[106]*FyRToe + z[37]*z[107]*
FyRHeel + z[37]*z[109]*FyRToe + z[82]*z[112]*FxLHeel + z[82]*z[112]*FxLToe +
z[83]*z[113]*FxLHeel + z[83]*z[115]*FxLToe + z[84]*z[112]*FyLHeel + z[84]*
z[112]*FyLToe + z[85]*z[113]*FyLHeel + z[85]*z[115]*FyLToe - z[101]*(z[36]*
z[43]+z[37]*z[44]) - z[101]*(z[84]*z[91]+z[85]*z[92]) - z[102]*(z[19]*z[23]-
z[20]*z[22]) - z[102]*(z[67]*z[71]-z[68]*z[70]);
z[339] = mom3 + z[331] + z[332] + par->ThighCMy*z[9]*z[103] + z[34]*z[106]*
FxRHeel + z[34]*z[106]*FxRToe + z[35]*z[107]*FxRHeel + z[35]*z[109]*FxRToe +
z[36]*z[106]*FyRHeel + z[36]*z[106]*FyRToe + z[37]*z[107]*FyRHeel + z[37]*
z[109]*FyRToe - z[101]*(z[36]*z[43]+z[37]*z[44]) - z[102]*(z[19]*z[23]-
z[20]*z[22]);
z[340] = mom3 + z[332] + par->ShankCMy*z[19]*z[102] + z[34]*z[105]*FxRHeel +
z[34]*z[105]*FxRToe + z[35]*z[108]*FxRHeel + z[35]*z[110]*FxRToe + z[36]*
z[105]*FyRHeel + z[36]*z[105]*FyRToe + z[37]*z[108]*FyRHeel + z[37]*z[110]*
FyRToe - z[101]*(z[36]*z[42]+z[37]*z[45]);
z[341] = mom3 + par->ContactHeelX*z[35]*FxRHeel + par->ContactHeelX*z[37]*
FyRHeel + par->ContactToeX*z[35]*FxRToe + par->ContactToeX*z[37]*FyRToe -
z[101]*(par->FootCMx*z[37]-par->FootCMy*z[36]) - par->ContactY*z[34]*FxRHeel -
par->ContactY*z[34]*FxRToe - par->ContactY*z[36]*FyRHeel - par->ContactY*
z[36]*FyRToe;
z[342] = mom6 + z[334] + z[335] + par->ThighCMy*z[57]*z[103] + z[82]*z[112]*
FxLHeel + z[82]*z[112]*FxLToe + z[83]*z[113]*FxLHeel + z[83]*z[115]*FxLToe +
z[84]*z[112]*FyLHeel + z[84]*z[112]*FyLToe + z[85]*z[113]*FyLHeel + z[85]*
z[115]*FyLToe - z[101]*(z[84]*z[91]+z[85]*z[92]) - z[102]*(z[67]*z[71]-
z[68]*z[70]);
z[343] = mom6 + z[335] + par->ShankCMy*z[67]*z[102] + z[82]*z[111]*FxLHeel +
z[82]*z[111]*FxLToe + z[83]*z[114]*FxLHeel + z[83]*z[116]*FxLToe + z[84]*
z[111]*FyLHeel + z[84]*z[111]*FyLToe + z[85]*z[114]*FyLHeel + z[85]*z[116]*
FyLToe - z[101]*(z[84]*z[90]+z[85]*z[93]);
z[344] = mom6 + par->ContactHeelX*z[83]*FxLHeel + par->ContactHeelX*z[85]*
FyLHeel + par->ContactToeX*z[83]*FxLToe + par->ContactToeX*z[85]*FyLToe -
z[101]*(par->FootCMx*z[85]-par->FootCMy*z[84]) - par->ContactY*z[82]*FxLHeel -
par->ContactY*z[82]*FxLToe - par->ContactY*z[84]*FyLHeel - par->ContactY*
z[84]*FyLToe;
z[345] = par->FootMass*(par->FootCMx*z[83]-par->FootCMy*z[82]);
z[346] = par->FootMass*(pow(z[34],2)+pow(z[35],2)) + par->FootMass*(pow(
z[82],2)+pow(z[83],2)) + par->ShankMass*(pow(z[17],2)+pow(z[18],2)) +
par->ShankMass*(pow(z[65],2)+pow(z[66],2)) + par->ThighMass*(pow(z[7],2)+
pow(z[8],2)) + par->ThighMass*(pow(z[55],2)+pow(z[56],2)) + par->TrunkMass*(
pow(z[1],2)+pow(z[2],2));
z[347] = par->ThighMass*z[7]*(z[8]+z[9]) + par->ThighMass*z[55]*(z[56]+
z[57]) + par->FootMass*(z[34]*z[36]+z[35]*z[37]) + par->FootMass*(z[82]*
z[84]+z[83]*z[85]) + par->ShankMass*(z[17]*z[19]+z[18]*z[20]) + par->ShankMass*(
z[65]*z[67]+z[66]*z[68]);
z[348] = par->FootMass*(z[34]*z[43]+z[35]*z[44]) + par->FootMass*(z[82]*
z[91]+z[83]*z[92]) + par->ShankMass*(z[17]*z[23]-z[18]*z[22]) + par->ShankMass*(
z[65]*z[71]-z[66]*z[70]) - par->ThighCMy*par->ThighMass*z[7] - par->ThighCMy*
par->ThighMass*z[55] - par->TrunkCMy*par->TrunkMass*z[1];
z[349] = par->FootMass*(z[82]*z[91]+z[83]*z[92]) + par->ShankMass*(z[65]*
z[71]-z[66]*z[70]) - par->ThighCMy*par->ThighMass*z[55];
z[350] = par->FootMass*(z[82]*z[90]+z[83]*z[93]) - par->ShankCMy*par->ShankMass*
z[65];
z[351] = par->FootMass*(par->FootCMx*z[35]-par->FootCMy*z[34]);
z[352] = par->FootMass*(z[34]*z[43]+z[35]*z[44]) + par->ShankMass*(z[17]*
z[23]-z[18]*z[22]) - par->ThighCMy*par->ThighMass*z[7];
z[353] = par->FootMass*(z[34]*z[42]+z[35]*z[45]) - par->ShankCMy*par->ShankMass*
z[17];
z[354] = par->TrunkMass*z[2]*z[4] + par->FootMass*(z[34]*z[51]+z[35]*z[52]) +
par->FootMass*(z[82]*z[99]+z[83]*z[100]) + par->ShankMass*(z[17]*z[24]+
z[18]*z[28]) + par->ShankMass*(z[65]*z[72]+z[66]*z[76]) - par->ThighMass*
z[8]*z[12] - par->ThighMass*z[56]*z[60];
z[355] = par->FootMass*(par->FootCMx*z[85]-par->FootCMy*z[84]);
z[356] = par->FootMass*(pow(z[36],2)+pow(z[37],2)) + par->FootMass*(pow(
z[84],2)+pow(z[85],2)) + par->ShankMass*(pow(z[19],2)+pow(z[20],2)) +
par->ShankMass*(pow(z[67],2)+pow(z[68],2)) + par->ThighMass*(pow(z[7],2)+
pow(z[9],2)) + par->ThighMass*(pow(z[55],2)+pow(z[57],2)) + par->TrunkMass*(
pow(z[1],2)+pow(z[2],2));
z[357] = par->FootMass*(z[36]*z[43]+z[37]*z[44]) + par->FootMass*(z[84]*
z[91]+z[85]*z[92]) + par->ShankMass*(z[19]*z[23]-z[20]*z[22]) + par->ShankMass*(
z[67]*z[71]-z[68]*z[70]) - par->ThighCMy*par->ThighMass*z[9] - par->ThighCMy*
par->ThighMass*z[57] - par->TrunkCMy*par->TrunkMass*z[2];
z[358] = par->FootMass*(z[84]*z[91]+z[85]*z[92]) + par->ShankMass*(z[67]*
z[71]-z[68]*z[70]) - par->ThighCMy*par->ThighMass*z[57];
z[359] = par->FootMass*(z[84]*z[90]+z[85]*z[93]) - par->ShankCMy*par->ShankMass*
z[67];
z[360] = par->FootMass*(par->FootCMx*z[37]-par->FootCMy*z[36]);
z[361] = par->FootMass*(z[36]*z[43]+z[37]*z[44]) + par->ShankMass*(z[19]*
z[23]-z[20]*z[22]) - par->ThighCMy*par->ThighMass*z[9];
z[362] = par->FootMass*(z[36]*z[42]+z[37]*z[45]) - par->ShankCMy*par->ShankMass*
z[19];
z[363] = par->FootMass*(z[36]*z[51]+z[37]*z[52]) + par->FootMass*(z[84]*
z[99]+z[85]*z[100]) + par->ShankMass*(z[19]*z[24]+z[20]*z[28]) + par->ShankMass*(
z[67]*z[72]+z[68]*z[76]) - par->ThighMass*z[7]*z[12] - par->ThighMass*z[55]*
z[60] - par->TrunkMass*z[1]*z[4];
z[364] = par->TrunkInertia + 2*par->FootInertia + 2*par->ShankInertia + 2*
par->ThighInertia + par->TrunkMass*pow(par->TrunkCMy,2) + 2*par->ThighMass*
pow(par->ThighCMy,2) + par->FootMass*(pow(z[43],2)+pow(z[44],2)) + par->FootMass*(
pow(z[91],2)+pow(z[92],2)) + par->ShankMass*(pow(z[22],2)+pow(z[23],2)) +
par->ShankMass*(pow(z[70],2)+pow(z[71],2));
z[365] = par->FootInertia + par->ShankInertia + par->ThighInertia + par->ThighMass*
pow(par->ThighCMy,2) + par->FootMass*(pow(z[91],2)+pow(z[92],2)) + par->ShankMass*(
pow(z[70],2)+pow(z[71],2));
z[366] = par->FootInertia + par->ShankInertia + par->FootMass*(z[90]*z[91]+
z[92]*z[93]) - par->ShankCMy*par->ShankMass*z[71];
z[367] = par->FootInertia + par->FootMass*(par->FootCMx*z[92]-par->FootCMy*
z[91]);
z[368] = par->FootInertia + par->ShankInertia + par->ThighInertia + par->ThighMass*
pow(par->ThighCMy,2) + par->FootMass*(pow(z[43],2)+pow(z[44],2)) + par->ShankMass*(
pow(z[22],2)+pow(z[23],2));
z[369] = par->FootInertia + par->ShankInertia + par->FootMass*(z[42]*z[43]+
z[44]*z[45]) - par->ShankCMy*par->ShankMass*z[23];
z[370] = par->FootInertia + par->FootMass*(par->FootCMx*z[44]-par->FootCMy*
z[43]);
z[371] = par->FootMass*(z[43]*z[51]+z[44]*z[52]) + par->FootMass*(z[91]*
z[99]+z[92]*z[100]) - par->ShankMass*(z[22]*z[28]-z[23]*z[24]) - par->ShankMass*(
z[70]*z[76]-z[71]*z[72]);
z[372] = par->FootMass*(z[43]*z[51]+z[44]*z[52]) - par->ShankMass*(z[22]*
z[28]-z[23]*z[24]);
z[373] = par->FootInertia + par->ShankInertia + par->ShankMass*pow(par->ShankCMy,
2) + par->FootMass*(pow(z[42],2)+pow(z[45],2));
z[374] = par->FootInertia + par->FootMass*(par->FootCMx*z[45]-par->FootCMy*
z[42]);
z[375] = par->FootMass*(z[42]*z[51]+z[45]*z[52]) - par->ShankCMy*par->ShankMass*
z[24];
z[376] = par->FootInertia + par->FootMass*(pow(par->FootCMx,2)+pow(par->FootCMy,
2));
z[377] = par->FootMass*(par->FootCMx*z[52]-par->FootCMy*z[51]);
z[378] = par->FootMass*(z[91]*z[99]+z[92]*z[100]) - par->ShankMass*(z[70]*
z[76]-z[71]*z[72]);
z[379] = par->FootInertia + par->ShankInertia + par->ShankMass*pow(par->ShankCMy,
2) + par->FootMass*(pow(z[90],2)+pow(z[93],2));
z[380] = par->FootInertia + par->FootMass*(par->FootCMx*z[93]-par->FootCMy*
z[90]);
z[381] = par->FootMass*(z[90]*z[99]+z[93]*z[100]) - par->ShankCMy*par->ShankMass*
z[72];
z[382] = par->FootMass*(par->FootCMx*z[100]-par->FootCMy*z[99]);
z[383] = z[34]*z[36]*z[117] + z[34]*z[36]*z[119] + z[35]*z[37]*z[117] +
z[35]*z[37]*z[119] + z[82]*z[84]*z[173] + z[82]*z[84]*z[175] + z[83]*z[85]*
z[173] + z[83]*z[85]*z[175] - pow(z[34],2)*z[118] - pow(z[34],2)*z[120] -
pow(z[35],2)*z[118] - pow(z[35],2)*z[120] - pow(z[82],2)*z[174] - pow(z[82],
2)*z[176] - pow(z[83],2)*z[174] - pow(z[83],2)*z[176];
z[384] = z[34]*z[36]*z[129] + z[34]*z[36]*z[135] + pow(z[34],2)*FyRHeel +
pow(z[34],2)*FyRToe + z[35]*z[37]*z[129] + z[35]*z[37]*z[135] + pow(z[35],2)*
FyRHeel + pow(z[35],2)*FyRToe + z[36]*z[127]*FyRHeel + z[36]*z[127]*FyRToe +
z[37]*z[126]*FyRHeel + z[37]*z[126]*FyRToe + z[82]*z[84]*z[185] + z[82]*
z[84]*z[191] + pow(z[82],2)*FyLHeel + pow(z[82],2)*FyLToe + z[83]*z[85]*
z[185] + z[83]*z[85]*z[191] + pow(z[83],2)*FyLHeel + pow(z[83],2)*FyLToe +
z[84]*z[183]*FyLHeel + z[84]*z[183]*FyLToe + z[85]*z[182]*FyLHeel + z[85]*
z[182]*FyLToe + 2*z[34]*z[127]*FxRHeel + 2*z[34]*z[127]*FxRToe + 2*z[35]*
z[126]*FxRHeel + 2*z[35]*z[126]*FxRToe + 2*z[82]*z[183]*FxLHeel + 2*z[82]*
z[183]*FxLToe + 2*z[83]*z[182]*FxLHeel + 2*z[83]*z[182]*FxLToe - z[7]*
z[103]*(z[7]+z[123]) - z[8]*z[103]*(z[8]+z[9]) - z[55]*z[103]*(z[55]+z[179]) -
z[56]*z[103]*(z[56]+z[57]) - z[101]*(pow(z[34],2)+pow(z[35],2)+z[36]*z[127]+
z[37]*z[126]) - z[101]*(pow(z[82],2)+pow(z[83],2)+z[84]*z[183]+z[85]*z[182]) -
z[102]*(pow(z[17],2)+pow(z[18],2)+z[19]*z[125]+z[20]*z[124]) - z[102]*(pow(
z[65],2)+pow(z[66],2)+z[67]*z[181]+z[68]*z[180]) - pow(z[34],2)*z[131] -
pow(z[34],2)*z[137] - pow(z[35],2)*z[131] - pow(z[35],2)*z[137] - pow(z[82],
2)*z[187] - pow(z[82],2)*z[193] - pow(z[83],2)*z[187] - pow(z[83],2)*z[193];
z[385] = par->TrunkMass*z[1]*z[4] + par->FootMass*(z[126]*z[52]+z[127]*
z[51]) + par->FootMass*(z[182]*z[100]+z[183]*z[99]) + par->ShankMass*(
z[124]*z[28]+z[125]*z[24]) + par->ShankMass*(z[180]*z[76]+z[181]*z[72]) -
par->ThighMass*z[123]*z[12] - par->ThighMass*z[179]*z[60];
z[386] = par->FootMass*(par->FootCMx*z[182]-par->FootCMy*z[183]);
z[387] = 2*par->ThighMass*z[8]*(z[7]+z[123]) + 2*par->ThighMass*z[56]*(
z[55]+z[179]) + 2*par->FootMass*(z[34]*z[127]+z[35]*z[126]) + 2*par->FootMass*(
z[82]*z[183]+z[83]*z[182]) + 2*par->ShankMass*(z[17]*z[125]+z[18]*z[124]) +
2*par->ShankMass*(z[65]*z[181]+z[66]*z[180]);
z[388] = par->ThighMass*z[7]*(z[7]+z[123]) + par->ThighMass*z[8]*(z[8]+z[9]) +
par->ThighMass*z[55]*(z[55]+z[179]) + par->ThighMass*z[56]*(z[56]+z[57]) +
par->FootMass*(pow(z[34],2)+pow(z[35],2)+z[36]*z[127]+z[37]*z[126]) +
par->FootMass*(pow(z[82],2)+pow(z[83],2)+z[84]*z[183]+z[85]*z[182]) +
par->ShankMass*(pow(z[17],2)+pow(z[18],2)+z[19]*z[125]+z[20]*z[124]) +
par->ShankMass*(pow(z[65],2)+pow(z[66],2)+z[67]*z[181]+z[68]*z[180]);
z[389] = par->TrunkCMy*par->TrunkMass*z[2] + par->FootMass*(z[43]*z[127]+
z[44]*z[126]) + par->FootMass*(z[91]*z[183]+z[92]*z[182]) - par->ThighCMy*
par->ThighMass*z[8] - par->ThighCMy*par->ThighMass*z[56] - par->ShankMass*(
z[22]*z[124]-z[23]*z[125]) - par->ShankMass*(z[70]*z[180]-z[71]*z[181]);
z[390] = par->FootMass*(z[91]*z[183]+z[92]*z[182]) - par->ThighCMy*par->ThighMass*
z[56] - par->ShankMass*(z[70]*z[180]-z[71]*z[181]);
z[391] = par->FootMass*(z[90]*z[183]+z[93]*z[182]) - par->ShankCMy*par->ShankMass*
z[181];
z[392] = par->FootMass*(par->FootCMx*z[126]-par->FootCMy*z[127]);
z[393] = par->FootMass*(z[43]*z[127]+z[44]*z[126]) - par->ThighCMy*par->ThighMass*
z[8] - par->ShankMass*(z[22]*z[124]-z[23]*z[125]);
z[394] = par->FootMass*(z[42]*z[127]+z[45]*z[126]) - par->ShankCMy*par->ShankMass*
z[125];
z[395] = z[34]*z[36]*z[129] + z[34]*z[36]*z[135] + pow(z[34],2)*FyRHeel +
pow(z[34],2)*FyRToe + z[35]*z[37]*z[129] + z[35]*z[37]*z[135] + pow(z[35],2)*
FyRHeel + pow(z[35],2)*FyRToe + z[36]*z[127]*FyRHeel + z[36]*z[127]*FyRToe +
z[37]*z[126]*FyRHeel + z[37]*z[126]*FyRToe + 2*z[34]*z[127]*FxRHeel + 2*
z[34]*z[127]*FxRToe + 2*z[35]*z[126]*FxRHeel + 2*z[35]*z[126]*FxRToe - z[7]*
z[103]*(z[7]+z[123]) - z[8]*z[103]*(z[8]+z[9]) - z[101]*(pow(z[34],2)+pow(
z[35],2)+z[36]*z[127]+z[37]*z[126]) - z[102]*(pow(z[17],2)+pow(z[18],2)+
z[19]*z[125]+z[20]*z[124]) - pow(z[34],2)*z[131] - pow(z[34],2)*z[137] -
pow(z[35],2)*z[131] - pow(z[35],2)*z[137];
z[396] = par->FootMass*(z[126]*z[52]+z[127]*z[51]) + par->ShankMass*(z[124]*
z[28]+z[125]*z[24]) - par->ThighMass*z[123]*z[12];
z[397] = 2*par->ThighMass*z[8]*(z[7]+z[123]) + 2*par->FootMass*(z[34]*
z[127]+z[35]*z[126]) + 2*par->ShankMass*(z[17]*z[125]+z[18]*z[124]);
z[398] = par->ThighMass*z[7]*(z[7]+z[123]) + par->ThighMass*z[8]*(z[8]+z[9]) +
par->FootMass*(pow(z[34],2)+pow(z[35],2)+z[36]*z[127]+z[37]*z[126]) +
par->ShankMass*(pow(z[17],2)+pow(z[18],2)+z[19]*z[125]+z[20]*z[124]);
z[399] = z[34]*z[36]*z[149] + z[34]*z[36]*z[155] + z[34]*z[139]*FyRHeel +
z[34]*z[139]*FyRToe + z[35]*z[37]*z[149] + z[35]*z[37]*z[155] + z[35]*
z[140]*FyRHeel + z[35]*z[140]*FyRToe + z[36]*z[146]*FyRHeel + z[36]*z[146]*
FyRToe + z[37]*z[144]*FyRHeel + z[37]*z[144]*FyRToe + 2*z[34]*z[146]*FxRHeel +
2*z[34]*z[146]*FxRToe + 2*z[35]*z[144]*FxRHeel + 2*z[35]*z[144]*FxRToe -
z[101]*(z[34]*z[139]+z[35]*z[140]+z[36]*z[146]+z[37]*z[144]) - z[102]*(
z[17]*z[20]+z[18]*z[19]+z[18]*z[138]+z[20]*z[143]) - pow(z[34],2)*z[151] -
pow(z[34],2)*z[157] - pow(z[35],2)*z[151] - pow(z[35],2)*z[157];
z[400] = (z[13]*z[29]-z[14]*z[30])*z[16];
z[401] = (z[13]*z[30]+z[14]*z[29])*z[16];
z[402] = par->ShankMass*(z[18]*z[24]+z[143]*z[28]+z[13]*z[17]*z[16]-z[14]*
z[18]*z[16]) - par->FootMass*(z[35]*z[401]-z[34]*z[400]-z[144]*z[52]-z[146]*
z[51]);
z[403] = 2*par->ShankMass*z[18]*(z[17]+z[143]) + 2*par->FootMass*(z[34]*
z[146]+z[35]*z[144]);
z[404] = par->FootMass*(z[34]*z[139]+z[35]*z[140]+z[36]*z[146]+z[37]*z[144]) +
par->ShankMass*(z[17]*z[20]+z[18]*z[19]+z[18]*z[138]+z[20]*z[143]);
z[405] = par->ShankMass*(z[18]*z[23]-z[22]*z[143]-par->ThighLen*z[13]*z[18]-
par->ThighLen*z[14]*z[17]) - par->FootMass*(z[34]*z[147]+z[35]*z[145]-z[43]*
z[146]-z[44]*z[144]);
z[406] = par->FootMass*(par->FootCMx*z[144]-par->FootCMy*z[146]);
z[407] = par->FootMass*(z[42]*z[146]+z[45]*z[144]) - par->ShankCMy*par->ShankMass*
z[18];
z[408] = z[34]*z[36]*z[164] + z[34]*z[36]*z[170] + z[34]*z[37]*FyRHeel +
z[34]*z[37]*FyRToe + z[35]*z[36]*FyRHeel + z[35]*z[36]*FyRToe + z[35]*z[37]*
z[164] + z[35]*z[37]*z[170] + z[35]*z[158]*FyRHeel + z[35]*z[158]*FyRToe +
z[37]*z[161]*FyRHeel + z[37]*z[161]*FyRToe + 2*z[34]*z[35]*FxRHeel + 2*
z[34]*z[35]*FxRToe + 2*z[35]*z[161]*FxRHeel + 2*z[35]*z[161]*FxRToe -
z[101]*(z[34]*z[37]+z[35]*z[36]+z[35]*z[158]+z[37]*z[161]) - pow(z[34],2)*
z[166] - pow(z[34],2)*z[172] - pow(z[35],2)*z[166] - pow(z[35],2)*z[172];
z[409] = -z[29]*z[24] - z[30]*z[33];
z[410] = par->FootMass*(z[34]*z[47]+z[35]*z[51]+z[35]*z[409]+z[161]*z[52]);
z[411] = par->FootMass*z[35]*(z[34]+z[161]);
z[412] = par->FootMass*(z[34]*z[37]+z[35]*z[36]+z[35]*z[158]+z[37]*z[161]);
z[413] = par->FootMass*(z[34]*z[40]+z[35]*z[43]+z[35]*z[162]+z[44]*z[161]);
z[414] = par->FootMass*(par->FootCMx*z[161]-par->FootCMy*z[35]);
z[415] = par->FootMass*(z[35]*z[42]+z[45]*z[161]-par->ShankLen*z[29]*z[35]-
par->ShankLen*z[30]*z[34]);
z[416] = z[82]*z[84]*z[185] + z[82]*z[84]*z[191] + pow(z[82],2)*FyLHeel +
pow(z[82],2)*FyLToe + z[83]*z[85]*z[185] + z[83]*z[85]*z[191] + pow(z[83],2)*
FyLHeel + pow(z[83],2)*FyLToe + z[84]*z[183]*FyLHeel + z[84]*z[183]*FyLToe +
z[85]*z[182]*FyLHeel + z[85]*z[182]*FyLToe + 2*z[82]*z[183]*FxLHeel + 2*
z[82]*z[183]*FxLToe + 2*z[83]*z[182]*FxLHeel + 2*z[83]*z[182]*FxLToe -
z[55]*z[103]*(z[55]+z[179]) - z[56]*z[103]*(z[56]+z[57]) - z[101]*(pow(
z[82],2)+pow(z[83],2)+z[84]*z[183]+z[85]*z[182]) - z[102]*(pow(z[65],2)+
pow(z[66],2)+z[67]*z[181]+z[68]*z[180]) - pow(z[82],2)*z[187] - pow(z[82],2)*
z[193] - pow(z[83],2)*z[187] - pow(z[83],2)*z[193];
z[417] = par->FootMass*(z[182]*z[100]+z[183]*z[99]) + par->ShankMass*(
z[180]*z[76]+z[181]*z[72]) - par->ThighMass*z[179]*z[60];
z[418] = 2*par->ThighMass*z[56]*(z[55]+z[179]) + 2*par->FootMass*(z[82]*
z[183]+z[83]*z[182]) + 2*par->ShankMass*(z[65]*z[181]+z[66]*z[180]);
z[419] = par->ThighMass*z[55]*(z[55]+z[179]) + par->ThighMass*z[56]*(z[56]+
z[57]) + par->FootMass*(pow(z[82],2)+pow(z[83],2)+z[84]*z[183]+z[85]*z[182]) +
par->ShankMass*(pow(z[65],2)+pow(z[66],2)+z[67]*z[181]+z[68]*z[180]);
z[420] = z[82]*z[84]*z[205] + z[82]*z[84]*z[211] + z[82]*z[195]*FyLHeel +
z[82]*z[195]*FyLToe + z[83]*z[85]*z[205] + z[83]*z[85]*z[211] + z[83]*
z[196]*FyLHeel + z[83]*z[196]*FyLToe + z[84]*z[202]*FyLHeel + z[84]*z[202]*
FyLToe + z[85]*z[200]*FyLHeel + z[85]*z[200]*FyLToe + 2*z[82]*z[202]*FxLHeel +
2*z[82]*z[202]*FxLToe + 2*z[83]*z[200]*FxLHeel + 2*z[83]*z[200]*FxLToe -
z[101]*(z[82]*z[195]+z[83]*z[196]+z[84]*z[202]+z[85]*z[200]) - z[102]*(
z[65]*z[68]+z[66]*z[67]+z[66]*z[194]+z[68]*z[199]) - pow(z[82],2)*z[207] -
pow(z[82],2)*z[213] - pow(z[83],2)*z[207] - pow(z[83],2)*z[213];
z[421] = (z[61]*z[77]-z[62]*z[78])*z[64];
z[422] = (z[61]*z[78]+z[62]*z[77])*z[64];
z[423] = par->ShankMass*(z[66]*z[72]+z[199]*z[76]+z[61]*z[65]*z[64]-z[62]*
z[66]*z[64]) - par->FootMass*(z[83]*z[422]-z[82]*z[421]-z[200]*z[100]-
z[202]*z[99]);
z[424] = par->FootMass*(par->FootCMx*z[200]-par->FootCMy*z[202]);
z[425] = 2*par->ShankMass*z[66]*(z[65]+z[199]) + 2*par->FootMass*(z[82]*
z[202]+z[83]*z[200]);
z[426] = par->FootMass*(z[82]*z[195]+z[83]*z[196]+z[84]*z[202]+z[85]*z[200]) +
par->ShankMass*(z[65]*z[68]+z[66]*z[67]+z[66]*z[194]+z[68]*z[199]);
z[427] = par->ShankMass*(z[66]*z[71]-z[70]*z[199]-par->ThighLen*z[61]*z[66]-
par->ThighLen*z[62]*z[65]) - par->FootMass*(z[82]*z[203]+z[83]*z[201]-z[91]*
z[202]-z[92]*z[200]);
z[428] = par->FootMass*(z[90]*z[202]+z[93]*z[200]) - par->ShankCMy*par->ShankMass*
z[66];
z[429] = z[82]*z[84]*z[220] + z[82]*z[84]*z[226] + z[82]*z[85]*FyLHeel +
z[82]*z[85]*FyLToe + z[83]*z[84]*FyLHeel + z[83]*z[84]*FyLToe + z[83]*z[85]*
z[220] + z[83]*z[85]*z[226] + z[83]*z[214]*FyLHeel + z[83]*z[214]*FyLToe +
z[85]*z[217]*FyLHeel + z[85]*z[217]*FyLToe + 2*z[82]*z[83]*FxLHeel + 2*
z[82]*z[83]*FxLToe + 2*z[83]*z[217]*FxLHeel + 2*z[83]*z[217]*FxLToe -
z[101]*(z[82]*z[85]+z[83]*z[84]+z[83]*z[214]+z[85]*z[217]) - pow(z[82],2)*
z[222] - pow(z[82],2)*z[228] - pow(z[83],2)*z[222] - pow(z[83],2)*z[228];
z[430] = -z[77]*z[72] - z[78]*z[81];
z[431] = par->FootMass*(z[82]*z[95]+z[83]*z[99]+z[83]*z[430]+z[217]*z[100]);
z[432] = par->FootMass*(par->FootCMx*z[217]-par->FootCMy*z[83]);
z[433] = par->FootMass*z[83]*(z[82]+z[217]);
z[434] = par->FootMass*(z[82]*z[85]+z[83]*z[84]+z[83]*z[214]+z[85]*z[217]);
z[435] = par->FootMass*(z[82]*z[88]+z[83]*z[91]+z[83]*z[218]+z[92]*z[217]);
z[436] = par->FootMass*(z[83]*z[90]+z[93]*z[217]-par->ShankLen*z[77]*z[83]-
par->ShankLen*z[78]*z[82]);
z[437] = pow(z[36],2)*z[117] + pow(z[36],2)*z[119] + pow(z[37],2)*z[117] +
pow(z[37],2)*z[119] + pow(z[84],2)*z[173] + pow(z[84],2)*z[175] + pow(z[85],
2)*z[173] + pow(z[85],2)*z[175] - z[34]*z[36]*z[118] - z[34]*z[36]*z[120] -
z[35]*z[37]*z[118] - z[35]*z[37]*z[120] - z[82]*z[84]*z[174] - z[82]*z[84]*
z[176] - z[83]*z[85]*z[174] - z[83]*z[85]*z[176];
z[438] = pow(z[34],2)*FxRHeel + pow(z[34],2)*FxRToe + pow(z[35],2)*FxRHeel +
pow(z[35],2)*FxRToe + z[36]*z[127]*FxRHeel + z[36]*z[127]*FxRToe + pow(
z[36],2)*z[129] + pow(z[36],2)*z[135] + z[37]*z[126]*FxRHeel + z[37]*z[126]*
FxRToe + pow(z[37],2)*z[129] + pow(z[37],2)*z[135] + pow(z[82],2)*FxLHeel +
pow(z[82],2)*FxLToe + pow(z[83],2)*FxLHeel + pow(z[83],2)*FxLToe + z[84]*
z[183]*FxLHeel + z[84]*z[183]*FxLToe + pow(z[84],2)*z[185] + pow(z[84],2)*
z[191] + z[85]*z[182]*FxLHeel + z[85]*z[182]*FxLToe + pow(z[85],2)*z[185] +
pow(z[85],2)*z[191] + 2*z[34]*z[36]*FyRHeel + 2*z[34]*z[36]*FyRToe + 2*
z[35]*z[37]*FyRHeel + 2*z[35]*z[37]*FyRToe + 2*z[82]*z[84]*FyLHeel + 2*
z[82]*z[84]*FyLToe + 2*z[83]*z[85]*FyLHeel + 2*z[83]*z[85]*FyLToe - 2*z[7]*
z[103]*(z[8]+z[9]) - 2*z[55]*z[103]*(z[56]+z[57]) - 2*z[101]*(z[34]*z[36]+
z[35]*z[37]) - 2*z[101]*(z[82]*z[84]+z[83]*z[85]) - 2*z[102]*(z[17]*z[19]+
z[18]*z[20]) - 2*z[102]*(z[65]*z[67]+z[66]*z[68]) - z[34]*z[36]*z[131] -
z[34]*z[36]*z[137] - z[35]*z[37]*z[131] - z[35]*z[37]*z[137] - z[82]*z[84]*
z[187] - z[82]*z[84]*z[193] - z[83]*z[85]*z[187] - z[83]*z[85]*z[193];
z[439] = 2*par->ThighMass*z[7]*(z[8]+z[9]) + 2*par->ThighMass*z[55]*(z[56]+
z[57]) + 2*par->FootMass*(z[34]*z[36]+z[35]*z[37]) + 2*par->FootMass*(z[82]*
z[84]+z[83]*z[85]) + 2*par->ShankMass*(z[17]*z[19]+z[18]*z[20]) + 2*par->ShankMass*(
z[65]*z[67]+z[66]*z[68]);
z[440] = pow(z[34],2)*FxRHeel + pow(z[34],2)*FxRToe + pow(z[35],2)*FxRHeel +
pow(z[35],2)*FxRToe + z[36]*z[127]*FxRHeel + z[36]*z[127]*FxRToe + pow(
z[36],2)*z[129] + pow(z[36],2)*z[135] + z[37]*z[126]*FxRHeel + z[37]*z[126]*
FxRToe + pow(z[37],2)*z[129] + pow(z[37],2)*z[135] + 2*z[34]*z[36]*FyRHeel +
2*z[34]*z[36]*FyRToe + 2*z[35]*z[37]*FyRHeel + 2*z[35]*z[37]*FyRToe - 2*
z[7]*z[103]*(z[8]+z[9]) - 2*z[101]*(z[34]*z[36]+z[35]*z[37]) - 2*z[102]*(
z[17]*z[19]+z[18]*z[20]) - z[34]*z[36]*z[131] - z[34]*z[36]*z[137] - z[35]*
z[37]*z[131] - z[35]*z[37]*z[137];
z[441] = par->FootMass*(z[34]*z[51]+z[35]*z[52]) + par->ShankMass*(z[17]*
z[24]+z[18]*z[28]) - par->ThighMass*z[8]*z[12];
z[442] = 2*par->ThighMass*z[7]*(z[8]+z[9]) + 2*par->FootMass*(z[34]*z[36]+
z[35]*z[37]) + 2*par->ShankMass*(z[17]*z[19]+z[18]*z[20]);
z[443] = z[34]*z[139]*FxRHeel + z[34]*z[139]*FxRToe + z[35]*z[140]*FxRHeel +
z[35]*z[140]*FxRToe + z[36]*z[146]*FxRHeel + z[36]*z[146]*FxRToe + pow(
z[36],2)*z[149] + pow(z[36],2)*z[155] + z[37]*z[144]*FxRHeel + z[37]*z[144]*
FxRToe + pow(z[37],2)*z[149] + pow(z[37],2)*z[155] + 2*z[36]*z[139]*FyRHeel +
2*z[36]*z[139]*FyRToe + 2*z[37]*z[140]*FyRHeel + 2*z[37]*z[140]*FyRToe - 2*
z[20]*z[102]*(z[19]+z[138]) - 2*z[101]*(z[36]*z[139]+z[37]*z[140]) - z[34]*
z[36]*z[151] - z[34]*z[36]*z[157] - z[35]*z[37]*z[151] - z[35]*z[37]*z[157];
z[444] = par->ShankMass*(z[20]*z[24]+z[138]*z[28]+z[13]*z[19]*z[16]-z[14]*
z[20]*z[16]) - par->FootMass*(z[37]*z[401]-z[36]*z[400]-z[139]*z[51]-z[140]*
z[52]);
z[445] = 2*par->ShankMass*z[20]*(z[19]+z[138]) + 2*par->FootMass*(z[36]*
z[139]+z[37]*z[140]);
z[446] = par->ShankMass*(z[20]*z[23]-z[22]*z[138]-par->ThighLen*z[13]*z[20]-
par->ThighLen*z[14]*z[19]) - par->FootMass*(z[36]*z[147]+z[37]*z[145]-z[43]*
z[139]-z[44]*z[140]);
z[447] = par->FootMass*(par->FootCMx*z[140]-par->FootCMy*z[139]);
z[448] = par->FootMass*(z[42]*z[139]+z[45]*z[140]) - par->ShankCMy*par->ShankMass*
z[20];
z[449] = z[34]*z[37]*FxRHeel + z[34]*z[37]*FxRToe + z[35]*z[36]*FxRHeel +
z[35]*z[36]*FxRToe + z[35]*z[158]*FxRHeel + z[35]*z[158]*FxRToe + pow(z[36],
2)*z[164] + pow(z[36],2)*z[170] + z[37]*z[161]*FxRHeel + z[37]*z[161]*
FxRToe + pow(z[37],2)*z[164] + pow(z[37],2)*z[170] + 2*z[36]*z[37]*FyRHeel +
2*z[36]*z[37]*FyRToe + 2*z[37]*z[158]*FyRHeel + 2*z[37]*z[158]*FyRToe - 2*
z[37]*z[101]*(z[36]+z[158]) - z[34]*z[36]*z[166] - z[34]*z[36]*z[172] -
z[35]*z[37]*z[166] - z[35]*z[37]*z[172];
z[450] = par->FootMass*(z[36]*z[47]+z[37]*z[51]+z[37]*z[409]+z[158]*z[52]);
z[451] = par->FootMass*z[37]*(z[36]+z[158]);
z[452] = par->FootMass*(z[36]*z[40]+z[37]*z[43]+z[37]*z[162]+z[44]*z[158]);
z[453] = par->FootMass*(par->FootCMx*z[158]-par->FootCMy*z[37]);
z[454] = par->FootMass*(z[37]*z[42]+z[45]*z[158]-par->ShankLen*z[29]*z[37]-
par->ShankLen*z[30]*z[36]);
z[455] = pow(z[82],2)*FxLHeel + pow(z[82],2)*FxLToe + pow(z[83],2)*FxLHeel +
pow(z[83],2)*FxLToe + z[84]*z[183]*FxLHeel + z[84]*z[183]*FxLToe + pow(
z[84],2)*z[185] + pow(z[84],2)*z[191] + z[85]*z[182]*FxLHeel + z[85]*z[182]*
FxLToe + pow(z[85],2)*z[185] + pow(z[85],2)*z[191] + 2*z[82]*z[84]*FyLHeel +
2*z[82]*z[84]*FyLToe + 2*z[83]*z[85]*FyLHeel + 2*z[83]*z[85]*FyLToe - 2*
z[55]*z[103]*(z[56]+z[57]) - 2*z[101]*(z[82]*z[84]+z[83]*z[85]) - 2*z[102]*(
z[65]*z[67]+z[66]*z[68]) - z[82]*z[84]*z[187] - z[82]*z[84]*z[193] - z[83]*
z[85]*z[187] - z[83]*z[85]*z[193];
z[456] = par->FootMass*(z[82]*z[99]+z[83]*z[100]) + par->ShankMass*(z[65]*
z[72]+z[66]*z[76]) - par->ThighMass*z[56]*z[60];
z[457] = 2*par->ThighMass*z[55]*(z[56]+z[57]) + 2*par->FootMass*(z[82]*
z[84]+z[83]*z[85]) + 2*par->ShankMass*(z[65]*z[67]+z[66]*z[68]);
z[458] = z[82]*z[195]*FxLHeel + z[82]*z[195]*FxLToe + z[83]*z[196]*FxLHeel +
z[83]*z[196]*FxLToe + z[84]*z[202]*FxLHeel + z[84]*z[202]*FxLToe + pow(