-
Notifications
You must be signed in to change notification settings - Fork 65
/
Copy pathikfast61.cpp
executable file
·13533 lines (13121 loc) · 457 KB
/
ikfast61.cpp
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
/// autogenerated analytical inverse kinematics code from ikfast program part of OpenRAVE
/// \author Rosen Diankov
///
/// Licensed under the Apache License, Version 2.0 (the "License");
/// you may not use this file except in compliance with the License.
/// You may obtain a copy of the License at
/// http://www.apache.org/licenses/LICENSE-2.0
///
/// Unless required by applicable law or agreed to in writing, software
/// distributed under the License is distributed on an "AS IS" BASIS,
/// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
/// See the License for the specific language governing permissions and
/// limitations under the License.
///
/// ikfast version 0x10000049 generated on 2018-08-27 16:40:22.782787
/// To compile with gcc:
/// gcc -lstdc++ ik.cpp
/// To compile without any main function as a shared object (might need -llapack):
/// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared -Wl,-soname,libik.so -o libik.so ik.cpp
#define IKFAST_HAS_LIBRARY
#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
using namespace ikfast;
// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000049);
#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <complex>
#ifndef IKFAST_ASSERT
#include <stdexcept>
#include <sstream>
#include <iostream>
#ifdef _MSC_VER
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __FUNCDNAME__
#endif
#endif
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __func__
#endif
#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
#endif
#if defined(_MSC_VER)
#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
#else
#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
#endif
#define IK2PI ((IkReal)6.28318530717959)
#define IKPI ((IkReal)3.14159265358979)
#define IKPI_2 ((IkReal)1.57079632679490)
#ifdef _MSC_VER
#ifndef isnan
#define isnan _isnan
#endif
#ifndef isinf
#define isinf _isinf
#endif
//#ifndef isfinite
//#define isfinite _isfinite
//#endif
#endif // _MSC_VER
// lapack routines
extern "C" {
void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
}
using namespace std; // necessary to get std math routines
#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif
inline float IKabs(float f) { return fabsf(f); }
inline double IKabs(double f) { return fabs(f); }
inline float IKsqr(float f) { return f*f; }
inline double IKsqr(double f) { return f*f; }
inline float IKlog(float f) { return logf(f); }
inline double IKlog(double f) { return log(f); }
// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
#endif
// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
#ifndef IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
#endif
// minimum distance of separate solutions
#ifndef IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
#endif
// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they can deviate
#ifndef IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
#endif
inline float IKasin(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(-IKPI_2);
else if( f >= 1 ) return float(IKPI_2);
return asinf(f);
}
inline double IKasin(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -IKPI_2;
else if( f >= 1 ) return IKPI_2;
return asin(f);
}
// return positive value in [0,y)
inline float IKfmod(float x, float y)
{
while(x < 0) {
x += y;
}
return fmodf(x,y);
}
// return positive value in [0,y)
inline double IKfmod(double x, double y)
{
while(x < 0) {
x += y;
}
return fmod(x,y);
}
inline float IKacos(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(IKPI);
else if( f >= 1 ) return float(0);
return acosf(f);
}
inline double IKacos(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return IKPI;
else if( f >= 1 ) return 0;
return acos(f);
}
inline float IKsin(float f) { return sinf(f); }
inline double IKsin(double f) { return sin(f); }
inline float IKcos(float f) { return cosf(f); }
inline double IKcos(double f) { return cos(f); }
inline float IKtan(float f) { return tanf(f); }
inline double IKtan(double f) { return tan(f); }
inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
inline float IKatan2Simple(float fy, float fx) {
return atan2f(fy,fx);
}
inline float IKatan2(float fy, float fx) {
if( isnan(fy) ) {
IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
return float(IKPI_2);
}
else if( isnan(fx) ) {
return 0;
}
return atan2f(fy,fx);
}
inline double IKatan2Simple(double fy, double fx) {
return atan2(fy,fx);
}
inline double IKatan2(double fy, double fx) {
if( isnan(fy) ) {
IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
return IKPI_2;
}
else if( isnan(fx) ) {
return 0;
}
return atan2(fy,fx);
}
template <typename T>
struct CheckValue
{
T value;
bool valid;
};
template <typename T>
inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
{
CheckValue<T> ret;
ret.valid = false;
ret.value = 0;
if( !isnan(fy) && !isnan(fx) ) {
if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
ret.value = IKatan2Simple(fy,fx);
ret.valid = true;
}
}
return ret;
}
inline float IKsign(float f) {
if( f > 0 ) {
return float(1);
}
else if( f < 0 ) {
return float(-1);
}
return 0;
}
inline double IKsign(double f) {
if( f > 0 ) {
return 1.0;
}
else if( f < 0 ) {
return -1.0;
}
return 0;
}
template <typename T>
inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
{
CheckValue<T> ret;
ret.valid = true;
if( n == 0 ) {
ret.value = 1.0;
return ret;
}
else if( n == 1 )
{
ret.value = f;
return ret;
}
else if( n < 0 )
{
if( f == 0 )
{
ret.valid = false;
ret.value = (T)1.0e30;
return ret;
}
if( n == -1 ) {
ret.value = T(1.0)/f;
return ret;
}
}
int num = n > 0 ? n : -n;
if( num == 2 ) {
ret.value = f*f;
}
else if( num == 3 ) {
ret.value = f*f*f;
}
else {
ret.value = 1.0;
while(num>0) {
if( num & 1 ) {
ret.value *= f;
}
num >>= 1;
f *= f;
}
}
if( n < 0 ) {
ret.value = T(1.0)/ret.value;
}
return ret;
}
/// solves the forward kinematics equations.
/// \param pfree is an array specifying the free joints of the chain.
IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47;
x0=IKcos(j[0]);
x1=IKcos(j[1]);
x2=IKcos(j[2]);
x3=IKsin(j[1]);
x4=IKsin(j[2]);
x5=IKcos(j[3]);
x6=IKsin(j[3]);
x7=IKcos(j[5]);
x8=IKsin(j[5]);
x9=IKcos(j[4]);
x10=IKsin(j[0]);
x11=IKsin(j[4]);
x12=((0.39225)*x0);
x13=((0.09465)*x0);
x14=((1.0)*x6);
x15=((1.0)*x10);
x16=((0.09465)*x10);
x17=((0.0823)*x6);
x18=((1.0)*x0);
x19=((0.39225)*x4);
x20=((0.425)*x1);
x21=((0.0823)*x5);
x22=((1.0)*x9);
x23=((1.0)*x5);
x24=(x1*x2);
x25=(x10*x9);
x26=(x1*x4);
x27=(x2*x3);
x28=(x3*x4);
x29=(x18*x28);
x30=(x15*x28);
x31=(x26+x27);
x32=((((-1.0)*x28))+(((1.0)*x24)));
x33=(x31*x5);
x34=(x32*x6);
x35=(((x0*x24))+(((-1.0)*x29)));
x36=((((-1.0)*x30))+((x10*x24)));
x37=(x18*(((((-1.0)*x26))+(((-1.0)*x27)))));
x38=(x15*(((((-1.0)*x26))+(((-1.0)*x27)))));
x39=(x35*x5);
x40=(x36*x5);
x41=(x37*x6);
x42=(((x32*x5))+((x6*(((((-1.0)*x26))+(((-1.0)*x27)))))));
x43=(x39+x41);
x44=(x40+((x38*x6)));
x45=(((x37*x5))+((x6*(((((-1.0)*x18*x24))+x29)))));
x46=(((x6*(((((-1.0)*x15*x24))+x30))))+((x38*x5)));
x47=(x43*x9);
eerot[0]=(((x7*((x47+((x10*x11))))))+((x45*x8)));
eerot[1]=(((x8*(((((-1.0)*x11*x15))+(((-1.0)*x22*x43))))))+((x45*x7)));
eerot[2]=(((x11*(((((-1.0)*x14*x37))+(((-1.0)*x23*x35))))))+x25);
eetrans[0]=(((x5*((((x13*x27))+((x13*x26))))))+(((0.10915)*x10))+((x6*(((((-1.0)*x13*x28))+((x13*x24))))))+(((-1.0)*x0*x20))+(((-1.0)*x12*x24))+((x12*x28))+(((0.0823)*x25))+((x11*(((((-1.0)*x17*x37))+(((-1.0)*x21*x35)))))));
eerot[3]=(((x46*x8))+((x7*(((((-1.0)*x0*x11))+((x44*x9)))))));
eerot[4]=(((x46*x7))+((x8*(((((-1.0)*x22*x44))+((x11*x18)))))));
eerot[5]=((((-1.0)*x18*x9))+((x11*(((((-1.0)*x14*x38))+(((-1.0)*x23*x36)))))));
eetrans[1]=((((-0.0823)*x0*x9))+(((-0.39225)*x10*x24))+((x5*((((x16*x26))+((x16*x27))))))+(((-0.10915)*x0))+(((-1.0)*x10*x20))+((x6*(((((-1.0)*x16*x28))+((x16*x24))))))+((x10*x19*x3))+((x11*(((((-1.0)*x17*x38))+(((-1.0)*x21*x36)))))));
eerot[6]=(((x7*x9*((x33+x34))))+((x42*x8)));
eerot[7]=(((x8*x9*(((((-1.0)*x34))+(((-1.0)*x33))))))+((x42*x7)));
eerot[8]=(x11*(((((-1.0)*x14*x32))+(((-1.0)*x23*x31)))));
eetrans[2]=((0.089159)+(((-1.0)*x1*x19))+((x6*(((((0.09465)*x26))+(((0.09465)*x27))))))+(((-0.425)*x3))+((x11*(((((-1.0)*x17*x32))+(((-1.0)*x21*x31))))))+((x5*(((((-0.09465)*x24))+(((0.09465)*x28))))))+(((-0.39225)*x27)));
}
IKFAST_API int GetNumFreeParameters() { return 0; }
IKFAST_API int* GetFreeParameters() { return NULL; }
IKFAST_API int GetNumJoints() { return 6; }
IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
IKFAST_API int GetIkType() { return 0x67000001; }
class IKSolver {
public:
IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
IkReal j100, cj100, sj100;
unsigned char _ij100[2], _nj100;
bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
solutions.Clear();
r00 = eerot[0*3+0];
r01 = eerot[0*3+1];
r02 = eerot[0*3+2];
r10 = eerot[1*3+0];
r11 = eerot[1*3+1];
r12 = eerot[1*3+2];
r20 = eerot[2*3+0];
r21 = eerot[2*3+1];
r22 = eerot[2*3+2];
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
new_r00=r00;
new_r01=r01;
new_r02=r02;
new_px=(px+(((-0.0823)*r02)));
new_r10=r10;
new_r11=r11;
new_r12=r12;
new_py=((((-0.0823)*r12))+py);
new_r20=r20;
new_r21=r21;
new_r22=r22;
new_pz=((-0.089159)+pz+(((-0.0823)*r22)));
r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
IkReal x48=((1.0)*px);
IkReal x49=((1.0)*pz);
IkReal x50=((1.0)*py);
pp=((px*px)+(py*py)+(pz*pz));
npx=(((px*r00))+((py*r10))+((pz*r20)));
npy=(((px*r01))+((py*r11))+((pz*r21)));
npz=(((px*r02))+((py*r12))+((pz*r22)));
rxp0_0=((((-1.0)*r20*x50))+((pz*r10)));
rxp0_1=(((px*r20))+(((-1.0)*r00*x49)));
rxp0_2=((((-1.0)*r10*x48))+((py*r00)));
rxp1_0=((((-1.0)*r21*x50))+((pz*r11)));
rxp1_1=(((px*r21))+(((-1.0)*r01*x49)));
rxp1_2=((((-1.0)*r11*x48))+((py*r01)));
rxp2_0=(((pz*r12))+(((-1.0)*r22*x50)));
rxp2_1=((((-1.0)*r02*x49))+((px*r22)));
rxp2_2=((((-1.0)*r12*x48))+((py*r02)));
IkReal IKFAST_ALIGNED16(matrixinvcoeffs[256]);
IkReal x51=((0.1893)*npx);
IkReal x52=((0.1893)*npy);
IkReal x53=((0.09465)*r22);
IkReal x54=((0.09465)*r21);
IkReal x55=((-0.09465)*r20);
IkReal x56=((-1.0)*r20);
IkReal x57=((-1.0)*rxp0_2);
IkReal x58=((-0.09465)*r21);
IkReal x59=((-1.0)*r21);
IkReal x60=((-1.0)*rxp1_2);
IkReal x61=((-1.0)*npx);
IkReal x62=((-1.0)*r22);
IkReal x63=((-1.0)*npz);
matrixinvcoeffs[0]=x55;
matrixinvcoeffs[1]=x51;
matrixinvcoeffs[2]=0;
matrixinvcoeffs[3]=0;
matrixinvcoeffs[4]=0;
matrixinvcoeffs[5]=0;
matrixinvcoeffs[6]=0;
matrixinvcoeffs[7]=0;
matrixinvcoeffs[8]=0;
matrixinvcoeffs[9]=npx;
matrixinvcoeffs[10]=x56;
matrixinvcoeffs[11]=0;
matrixinvcoeffs[12]=0;
matrixinvcoeffs[13]=0;
matrixinvcoeffs[14]=0;
matrixinvcoeffs[15]=x57;
matrixinvcoeffs[16]=0;
matrixinvcoeffs[17]=0;
matrixinvcoeffs[18]=x55;
matrixinvcoeffs[19]=x51;
matrixinvcoeffs[20]=0;
matrixinvcoeffs[21]=0;
matrixinvcoeffs[22]=0;
matrixinvcoeffs[23]=0;
matrixinvcoeffs[24]=npx;
matrixinvcoeffs[25]=0;
matrixinvcoeffs[26]=0;
matrixinvcoeffs[27]=x56;
matrixinvcoeffs[28]=0;
matrixinvcoeffs[29]=0;
matrixinvcoeffs[30]=x57;
matrixinvcoeffs[31]=0;
matrixinvcoeffs[32]=x58;
matrixinvcoeffs[33]=x52;
matrixinvcoeffs[34]=0;
matrixinvcoeffs[35]=0;
matrixinvcoeffs[36]=0;
matrixinvcoeffs[37]=0;
matrixinvcoeffs[38]=0;
matrixinvcoeffs[39]=0;
matrixinvcoeffs[40]=0;
matrixinvcoeffs[41]=npy;
matrixinvcoeffs[42]=x59;
matrixinvcoeffs[43]=0;
matrixinvcoeffs[44]=0;
matrixinvcoeffs[45]=0;
matrixinvcoeffs[46]=0;
matrixinvcoeffs[47]=x60;
matrixinvcoeffs[48]=0;
matrixinvcoeffs[49]=0;
matrixinvcoeffs[50]=x58;
matrixinvcoeffs[51]=x52;
matrixinvcoeffs[52]=0;
matrixinvcoeffs[53]=0;
matrixinvcoeffs[54]=0;
matrixinvcoeffs[55]=0;
matrixinvcoeffs[56]=npy;
matrixinvcoeffs[57]=0;
matrixinvcoeffs[58]=0;
matrixinvcoeffs[59]=x59;
matrixinvcoeffs[60]=0;
matrixinvcoeffs[61]=0;
matrixinvcoeffs[62]=x60;
matrixinvcoeffs[63]=0;
matrixinvcoeffs[64]=0;
matrixinvcoeffs[65]=0;
matrixinvcoeffs[66]=0;
matrixinvcoeffs[67]=0;
matrixinvcoeffs[68]=0;
matrixinvcoeffs[69]=0;
matrixinvcoeffs[70]=0;
matrixinvcoeffs[71]=0;
matrixinvcoeffs[72]=npz;
matrixinvcoeffs[73]=0;
matrixinvcoeffs[74]=0;
matrixinvcoeffs[75]=x62;
matrixinvcoeffs[76]=x53;
matrixinvcoeffs[77]=0;
matrixinvcoeffs[78]=((-1.0)*rxp2_2);
matrixinvcoeffs[79]=0;
matrixinvcoeffs[80]=0;
matrixinvcoeffs[81]=0;
matrixinvcoeffs[82]=0;
matrixinvcoeffs[83]=0;
matrixinvcoeffs[84]=0;
matrixinvcoeffs[85]=0;
matrixinvcoeffs[86]=0;
matrixinvcoeffs[87]=0;
matrixinvcoeffs[88]=0;
matrixinvcoeffs[89]=x63;
matrixinvcoeffs[90]=r22;
matrixinvcoeffs[91]=0;
matrixinvcoeffs[92]=0;
matrixinvcoeffs[93]=x53;
matrixinvcoeffs[94]=0;
matrixinvcoeffs[95]=rxp2_2;
matrixinvcoeffs[96]=0;
matrixinvcoeffs[97]=0;
matrixinvcoeffs[98]=0;
matrixinvcoeffs[99]=0;
matrixinvcoeffs[100]=r21;
matrixinvcoeffs[101]=npy;
matrixinvcoeffs[102]=0;
matrixinvcoeffs[103]=0;
matrixinvcoeffs[104]=0;
matrixinvcoeffs[105]=0;
matrixinvcoeffs[106]=0;
matrixinvcoeffs[107]=0;
matrixinvcoeffs[108]=x60;
matrixinvcoeffs[109]=0;
matrixinvcoeffs[110]=x58;
matrixinvcoeffs[111]=0;
matrixinvcoeffs[112]=0;
matrixinvcoeffs[113]=0;
matrixinvcoeffs[114]=0;
matrixinvcoeffs[115]=0;
matrixinvcoeffs[116]=0;
matrixinvcoeffs[117]=0;
matrixinvcoeffs[118]=r21;
matrixinvcoeffs[119]=npy;
matrixinvcoeffs[120]=0;
matrixinvcoeffs[121]=0;
matrixinvcoeffs[122]=0;
matrixinvcoeffs[123]=0;
matrixinvcoeffs[124]=0;
matrixinvcoeffs[125]=x60;
matrixinvcoeffs[126]=0;
matrixinvcoeffs[127]=x54;
matrixinvcoeffs[128]=0;
matrixinvcoeffs[129]=0;
matrixinvcoeffs[130]=0;
matrixinvcoeffs[131]=0;
matrixinvcoeffs[132]=x56;
matrixinvcoeffs[133]=x61;
matrixinvcoeffs[134]=0;
matrixinvcoeffs[135]=0;
matrixinvcoeffs[136]=0;
matrixinvcoeffs[137]=0;
matrixinvcoeffs[138]=0;
matrixinvcoeffs[139]=0;
matrixinvcoeffs[140]=rxp0_2;
matrixinvcoeffs[141]=0;
matrixinvcoeffs[142]=((0.09465)*r20);
matrixinvcoeffs[143]=0;
matrixinvcoeffs[144]=0;
matrixinvcoeffs[145]=0;
matrixinvcoeffs[146]=0;
matrixinvcoeffs[147]=0;
matrixinvcoeffs[148]=0;
matrixinvcoeffs[149]=0;
matrixinvcoeffs[150]=x56;
matrixinvcoeffs[151]=x61;
matrixinvcoeffs[152]=0;
matrixinvcoeffs[153]=0;
matrixinvcoeffs[154]=0;
matrixinvcoeffs[155]=0;
matrixinvcoeffs[156]=0;
matrixinvcoeffs[157]=rxp0_2;
matrixinvcoeffs[158]=0;
matrixinvcoeffs[159]=x55;
matrixinvcoeffs[160]=0;
matrixinvcoeffs[161]=0;
matrixinvcoeffs[162]=0;
matrixinvcoeffs[163]=0;
matrixinvcoeffs[164]=x62;
matrixinvcoeffs[165]=x63;
matrixinvcoeffs[166]=0;
matrixinvcoeffs[167]=0;
matrixinvcoeffs[168]=0;
matrixinvcoeffs[169]=0;
matrixinvcoeffs[170]=0;
matrixinvcoeffs[171]=0;
matrixinvcoeffs[172]=rxp2_2;
matrixinvcoeffs[173]=0;
matrixinvcoeffs[174]=x53;
matrixinvcoeffs[175]=0;
matrixinvcoeffs[176]=0;
matrixinvcoeffs[177]=0;
matrixinvcoeffs[178]=0;
matrixinvcoeffs[179]=0;
matrixinvcoeffs[180]=0;
matrixinvcoeffs[181]=0;
matrixinvcoeffs[182]=x62;
matrixinvcoeffs[183]=x63;
matrixinvcoeffs[184]=0;
matrixinvcoeffs[185]=0;
matrixinvcoeffs[186]=0;
matrixinvcoeffs[187]=0;
matrixinvcoeffs[188]=0;
matrixinvcoeffs[189]=rxp2_2;
matrixinvcoeffs[190]=0;
matrixinvcoeffs[191]=((-0.09465)*r22);
matrixinvcoeffs[192]=0;
matrixinvcoeffs[193]=0;
matrixinvcoeffs[194]=0;
matrixinvcoeffs[195]=0;
matrixinvcoeffs[196]=0;
matrixinvcoeffs[197]=0;
matrixinvcoeffs[198]=0;
matrixinvcoeffs[199]=0;
matrixinvcoeffs[200]=npy;
matrixinvcoeffs[201]=0;
matrixinvcoeffs[202]=0;
matrixinvcoeffs[203]=x59;
matrixinvcoeffs[204]=x54;
matrixinvcoeffs[205]=0;
matrixinvcoeffs[206]=x60;
matrixinvcoeffs[207]=0;
matrixinvcoeffs[208]=0;
matrixinvcoeffs[209]=0;
matrixinvcoeffs[210]=0;
matrixinvcoeffs[211]=0;
matrixinvcoeffs[212]=0;
matrixinvcoeffs[213]=0;
matrixinvcoeffs[214]=0;
matrixinvcoeffs[215]=0;
matrixinvcoeffs[216]=0;
matrixinvcoeffs[217]=((-1.0)*npy);
matrixinvcoeffs[218]=r21;
matrixinvcoeffs[219]=0;
matrixinvcoeffs[220]=0;
matrixinvcoeffs[221]=x54;
matrixinvcoeffs[222]=0;
matrixinvcoeffs[223]=rxp1_2;
matrixinvcoeffs[224]=0;
matrixinvcoeffs[225]=0;
matrixinvcoeffs[226]=0;
matrixinvcoeffs[227]=0;
matrixinvcoeffs[228]=0;
matrixinvcoeffs[229]=0;
matrixinvcoeffs[230]=0;
matrixinvcoeffs[231]=0;
matrixinvcoeffs[232]=x61;
matrixinvcoeffs[233]=0;
matrixinvcoeffs[234]=0;
matrixinvcoeffs[235]=r20;
matrixinvcoeffs[236]=x55;
matrixinvcoeffs[237]=0;
matrixinvcoeffs[238]=rxp0_2;
matrixinvcoeffs[239]=0;
matrixinvcoeffs[240]=0;
matrixinvcoeffs[241]=0;
matrixinvcoeffs[242]=0;
matrixinvcoeffs[243]=0;
matrixinvcoeffs[244]=0;
matrixinvcoeffs[245]=0;
matrixinvcoeffs[246]=0;
matrixinvcoeffs[247]=0;
matrixinvcoeffs[248]=0;
matrixinvcoeffs[249]=npx;
matrixinvcoeffs[250]=x56;
matrixinvcoeffs[251]=0;
matrixinvcoeffs[252]=0;
matrixinvcoeffs[253]=x55;
matrixinvcoeffs[254]=0;
matrixinvcoeffs[255]=x57;
if( !matrixinverse<16>(matrixinvcoeffs) ) {
continue;
}
IkReal gclwh0_0=matrixinvcoeffs[0], gclwh0_1=matrixinvcoeffs[16], gclwh1_2=matrixinvcoeffs[33], gclwh1_3=matrixinvcoeffs[49], gclwh2_0=matrixinvcoeffs[2], gclwh2_1=matrixinvcoeffs[18], gclwh3_2=matrixinvcoeffs[35], gclwh3_3=matrixinvcoeffs[51], gclwh4_2=matrixinvcoeffs[36], gclwh4_3=matrixinvcoeffs[52], gclwh4_4=matrixinvcoeffs[68], gclwh4_8=matrixinvcoeffs[132], gclwh4_11=matrixinvcoeffs[180], gclwh4_14=matrixinvcoeffs[228], gclwh5_0=matrixinvcoeffs[5], gclwh5_1=matrixinvcoeffs[21], gclwh5_6=matrixinvcoeffs[101], gclwh5_9=matrixinvcoeffs[149], gclwh5_10=matrixinvcoeffs[165], gclwh5_15=matrixinvcoeffs[245], gclwh6_2=matrixinvcoeffs[38], gclwh6_4=matrixinvcoeffs[70], gclwh6_5=matrixinvcoeffs[86], gclwh6_11=matrixinvcoeffs[182], gclwh6_12=matrixinvcoeffs[198], gclwh7_0=matrixinvcoeffs[7], gclwh7_6=matrixinvcoeffs[103], gclwh7_7=matrixinvcoeffs[119], gclwh7_10=matrixinvcoeffs[167], gclwh7_13=matrixinvcoeffs[215], gclwh8_2=matrixinvcoeffs[40], gclwh8_4=matrixinvcoeffs[72], gclwh8_5=matrixinvcoeffs[88], gclwh8_11=matrixinvcoeffs[184], gclwh8_12=matrixinvcoeffs[200], gclwh9_0=matrixinvcoeffs[9], gclwh9_6=matrixinvcoeffs[105], gclwh9_7=matrixinvcoeffs[121], gclwh9_10=matrixinvcoeffs[169], gclwh9_13=matrixinvcoeffs[217], gclwh10_2=matrixinvcoeffs[42], gclwh10_4=matrixinvcoeffs[74], gclwh10_5=matrixinvcoeffs[90], gclwh10_11=matrixinvcoeffs[186], gclwh10_12=matrixinvcoeffs[202], gclwh11_0=matrixinvcoeffs[11], gclwh11_6=matrixinvcoeffs[107], gclwh11_7=matrixinvcoeffs[123], gclwh11_10=matrixinvcoeffs[171], gclwh11_13=matrixinvcoeffs[219], gclwh12_2=matrixinvcoeffs[44], gclwh12_3=matrixinvcoeffs[60], gclwh12_4=matrixinvcoeffs[76], gclwh12_8=matrixinvcoeffs[140], gclwh12_11=matrixinvcoeffs[188], gclwh12_14=matrixinvcoeffs[236], gclwh13_0=matrixinvcoeffs[13], gclwh13_1=matrixinvcoeffs[29], gclwh13_6=matrixinvcoeffs[109], gclwh13_9=matrixinvcoeffs[157], gclwh13_10=matrixinvcoeffs[173], gclwh13_15=matrixinvcoeffs[253], gclwh14_2=matrixinvcoeffs[46], gclwh14_3=matrixinvcoeffs[62], gclwh14_4=matrixinvcoeffs[78], gclwh14_8=matrixinvcoeffs[142], gclwh14_11=matrixinvcoeffs[190], gclwh14_14=matrixinvcoeffs[238], gclwh15_0=matrixinvcoeffs[15], gclwh15_1=matrixinvcoeffs[31], gclwh15_6=matrixinvcoeffs[111], gclwh15_9=matrixinvcoeffs[159], gclwh15_10=matrixinvcoeffs[175], gclwh15_15=matrixinvcoeffs[255];
IkReal op[72], zeror[48];
int numroots;;
IkReal x64=pz*pz;
IkReal x65=pp*pp;
IkReal x66=((0.1893)*pz);
IkReal x67=((1.7)*pz);
IkReal x68=(gclwh15_9*npx);
IkReal x69=(gclwh6_11*r21);
IkReal x70=(pp*r22);
IkReal x71=((0.00702803935125)*r21);
IkReal x72=(gclwh5_9*npz);
IkReal x73=(gclwh8_11*r20);
IkReal x74=(npy*pz);
IkReal x75=((3.60823078949063e-5)*r22);
IkReal x76=(gclwh14_3*rxp0_2);
IkReal x77=((0.000977833645875)*r20);
IkReal x78=(npz*pz);
IkReal x79=((0.000977833645875)*r21);
IkReal x80=(gclwh10_12*r22);
IkReal x81=(gclwh10_2*rxp2_2);
IkReal x82=((0.00076243651125)*rxp1_2);
IkReal x83=((0.00600991575645741)*r20);
IkReal x84=((0.154705425)*rxp1_2);
IkReal x85=((0.017917245)*r21);
IkReal x86=(gclwh6_11*rxp1_2);
IkReal x87=((0.3786)*rxp1_2);
IkReal x88=(gclwh7_0*rxp1_2);
IkReal x89=((0.7572)*rxp1_2);
IkReal x90=((0.154705425)*rxp2_2);
IkReal x91=((2.0)*gclwh11_10);
IkReal x92=(gclwh12_8*r21);
IkReal x93=((0.81725)*pp);
IkReal x94=(pp*pz);
IkReal x95=(gclwh14_2*r20);
IkReal x96=(pp*r21);
IkReal x97=((2.0)*gclwh4_11);
IkReal x98=(gclwh8_12*r20);
IkReal x99=(npx*pz);
IkReal x100=(gclwh14_8*rxp0_2);
IkReal x101=((0.1893)*rxp2_2);
IkReal x102=(gclwh5_9*rxp2_2);
IkReal x103=((0.020662095)*rxp1_2);
IkReal x104=((0.007321434238125)*r21);
IkReal x105=((1.6345)*pz);
IkReal x106=((0.0089586225)*pz);
IkReal x107=(gclwh13_0*r21);
IkReal x108=(gclwh5_9*r22);
IkReal x109=(gclwh11_10*r22);
IkReal x110=(gclwh11_13*r22);
IkReal x111=((1.0)*r21);
IkReal x112=((0.3786)*rxp0_2);
IkReal x113=(gclwh14_14*rxp0_2);
IkReal x114=(pp*r20);
IkReal x115=((0.00195566729175)*r22);
IkReal x116=(gclwh15_10*rxp0_2);
IkReal x117=(gclwh14_8*r20);
IkReal x118=(gclwh12_8*rxp1_2);
IkReal x119=((2.0)*gclwh9_10);
IkReal x120=((2.0)*gclwh15_10);
IkReal x121=(gclwh5_15*rxp2_2);
IkReal x122=((2.0)*gclwh13_10);
IkReal x123=((0.00702803935125)*r20);
IkReal x124=(gclwh13_9*r21);
IkReal x125=(gclwh5_0*r22);
IkReal x126=((0.01464286847625)*r20);
IkReal x127=(gclwh15_1*r20);
IkReal x128=((0.000977833645875)*r22);
IkReal x129=((0.0089586225)*r20);
IkReal x130=((2.0)*gclwh10_11);
IkReal x131=(gclwh5_10*r22);
IkReal x132=((2.0)*pp);
IkReal x133=(gclwh15_9*r20);
IkReal x134=(gclwh1_3*r20);
IkReal x135=((0.00195566729175)*r20);
IkReal x136=(gclwh4_3*rxp2_2);
IkReal x137=((0.2183)*gclwh13_15);
IkReal x138=((2.0)*gclwh7_10);
IkReal x139=(gclwh15_0*r20);
IkReal x140=(gclwh9_0*r20);
IkReal x141=(gclwh15_15*rxp0_2);
IkReal x142=(gclwh2_1*r21);
IkReal x143=((4.0)*gclwh11_10);
IkReal x144=((0.30941085)*rxp0_2);
IkReal x145=(gclwh9_0*rxp0_2);
IkReal x146=(gclwh5_10*rxp2_2);
IkReal x147=((0.2183)*gclwh12_14);
IkReal x148=((0.2183)*gclwh14_14);
IkReal x149=(gclwh4_2*rxp2_2);
IkReal x150=((0.03275)*pp);
IkReal x151=((0.006199575)*rxp1_2);
IkReal x152=(gclwh12_2*rxp1_2);
IkReal x153=(gclwh5_0*rxp2_2);
IkReal x154=((3.60823078949063e-5)*r21);
IkReal x155=(gclwh14_11*rxp0_2);
IkReal x156=((0.020662095)*rxp0_2);
IkReal x157=(gclwh14_8*npx);
IkReal x158=((4.0)*gclwh9_10);
IkReal x159=((0.00195566729175)*r21);
IkReal x160=(gclwh8_12*rxp0_2);
IkReal x161=((0.01464286847625)*r21);
IkReal x162=((0.3786)*rxp2_2);
IkReal x163=((0.01464286847625)*r22);
IkReal x164=(gclwh7_13*r21);
IkReal x165=((4.0)*gclwh7_10);
IkReal x166=(gclwh13_0*rxp1_2);
IkReal x167=((0.1893)*rxp1_2);
IkReal x168=(gclwh6_2*r21);
IkReal x169=(gclwh5_1*rxp2_2);
IkReal x170=(gclwh8_2*rxp0_2);
IkReal x171=(gclwh9_10*r20);
IkReal x172=((4.0)*pp);
IkReal x173=(gclwh15_1*rxp0_2);
IkReal x174=(gclwh9_13*r20);
IkReal x175=((0.0089586225)*r21);
IkReal x176=(gclwh0_0*r20);
IkReal x177=(gclwh10_11*r22);
IkReal x178=((2.0)*gclwh12_11);
IkReal x179=((0.0655)*pz);
IkReal x180=((0.006199575)*rxp2_2);
IkReal x181=(gclwh12_14*rxp1_2);
IkReal x182=(gclwh15_0*rxp0_2);
IkReal x183=((2.0)*npx);
IkReal x184=(gclwh9_13*rxp0_2);
IkReal x185=((1.0)*pp);
IkReal x186=((4.0)*gclwh10_11);
IkReal x187=(gclwh15_9*rxp0_2);
IkReal x188=((3.60823078949063e-5)*r20);
IkReal x189=((0.00702803935125)*r22);
IkReal x190=(gclwh8_2*r20);
IkReal x191=((0.2183)*gclwh5_15);
IkReal x192=(gclwh10_2*r22);
IkReal x193=(gclwh5_1*r22);
IkReal x194=(gclwh13_9*rxp1_2);
IkReal x195=((2.0)*gclwh5_10);
IkReal x196=((0.020662095)*rxp2_2);
IkReal x197=(gclwh4_8*r22);
IkReal x198=((0.017917245)*r20);
IkReal x199=(gclwh7_0*r21);
IkReal x200=((0.00600991575645741)*r21);
IkReal x201=(gclwh11_0*r22);
IkReal x202=(gclwh6_12*r21);
IkReal x203=((0.2183)*gclwh15_15);
IkReal x204=(gclwh11_0*rxp2_2);
IkReal x205=((0.2183)*gclwh4_14);
IkReal x206=(gclwh4_14*rxp2_2);
IkReal x207=(gclwh6_2*rxp1_2);
IkReal x208=((0.1893)*rxp0_2);
IkReal x209=(gclwh4_8*rxp2_2);
IkReal x210=((0.12699240901125)*rxp1_2);
IkReal x211=(gclwh14_2*rxp0_2);
IkReal x212=(gclwh4_3*r22);
IkReal x213=((0.1893)*pp);
IkReal x214=(gclwh13_15*rxp1_2);
IkReal x215=(gclwh1_2*r20);
IkReal x216=((2.0)*gclwh14_11);
IkReal x217=(gclwh4_11*r22);
IkReal x218=((0.020662095)*x113);
IkReal x219=(gclwh6_5*x79);
IkReal x220=(gclwh8_5*x77);
IkReal x221=(gclwh12_14*x103);
IkReal x222=(gclwh10_5*x128);
IkReal x223=(gclwh4_14*x196);
IkReal x224=((0.0089586225)*x69);
IkReal x225=(gclwh12_11*x167);
IkReal x226=((0.017917245)*x118);
IkReal x227=((0.017917245)*x209);
IkReal x228=(gclwh4_11*x101);
IkReal x229=((0.0089586225)*x73);
IkReal x230=((0.1893)*x155);
IkReal x231=((0.0089586225)*x177);
IkReal x232=((0.017917245)*x100);
IkReal x233=((0.017917245)*x102);
IkReal x234=((0.0089586225)*x109);
IkReal x235=((0.1893)*x116);
IkReal x236=((0.017917245)*x194);
IkReal x237=(gclwh9_10*x129);
IkReal x238=((0.017917245)*x187);
IkReal x239=(gclwh13_10*x167);
IkReal x240=(gclwh7_10*x175);
IkReal x241=(gclwh5_10*x101);
IkReal x242=((0.020662095)*x141);
IkReal x243=(gclwh7_7*x79);
IkReal x244=(gclwh9_7*x77);
IkReal x245=(gclwh13_15*x103);
IkReal x246=((0.020662095)*x121);
IkReal x247=(gclwh11_7*x128);
IkReal x248=(gclwh10_5*x196);
IkReal x249=(gclwh8_5*x156);
IkReal x250=(gclwh14_14*x77);
IkReal x251=(gclwh6_5*x103);
IkReal x252=(gclwh4_14*x128);
IkReal x253=(gclwh12_14*x79);
IkReal x254=(gclwh8_11*x208);
IkReal x255=((0.000847933619625)*x92);
IkReal x256=(gclwh12_11*x175);
IkReal x257=(gclwh10_11*x101);
IkReal x258=((0.0089586225)*x217);
IkReal x259=((0.000847933619625)*x197);
IkReal x260=((0.1893)*x86);
IkReal x261=(gclwh14_11*x129);
IkReal x262=((0.000847933619625)*x117);
IkReal x263=(gclwh9_10*x208);
IkReal x264=(gclwh13_10*x175);
IkReal x265=(gclwh11_10*x101);
IkReal x266=((0.000847933619625)*x133);
IkReal x267=((0.0089586225)*x131);
IkReal x268=((0.000847933619625)*x108);
IkReal x269=(gclwh7_10*x167);
IkReal x270=((0.000847933619625)*x124);
IkReal x271=(gclwh15_10*x129);
IkReal x272=(gclwh5_15*x128);
IkReal x273=(gclwh7_7*x103);
IkReal x274=(gclwh13_15*x79);
IkReal x275=(gclwh15_15*x77);
IkReal x276=(gclwh9_7*x156);
IkReal x277=(gclwh11_7*x196);
IkReal x278=((0.017917245)*x177);
IkReal x279=(gclwh14_11*x112);
IkReal x280=((0.017917245)*x73);
IkReal x281=((0.04132419)*x113);
IkReal x282=((0.04132419)*x181);
IkReal x283=((0.04132419)*x206);
IkReal x284=(gclwh4_11*x162);
IkReal x285=((0.017917245)*x69);
IkReal x286=(gclwh12_11*x87);
IkReal x287=((0.04132419)*x121);
IkReal x288=((0.04132419)*x214);
IkReal x289=((0.04132419)*x141);
IkReal x290=((0.017917245)*x171);
IkReal x291=(gclwh15_10*x112);
IkReal x292=((0.017917245)*x109);
IkReal x293=(gclwh7_10*x85);
IkReal x294=((0.3786)*x146);
IkReal x295=(gclwh13_10*x87);
IkReal x296=((0.3786)*x86);
IkReal x297=(gclwh14_11*x198);
IkReal x298=(gclwh14_14*x135);
IkReal x299=(gclwh4_14*x115);
IkReal x300=(gclwh12_14*x159);
IkReal x301=(gclwh14_8*x114);
IkReal x302=((2.0)*x64);
IkReal x303=((1.0)*x65);
IkReal x304=(r21*x65);
IkReal x305=((0.2183)*gclwh10_5*x78);
IkReal x306=((0.2183)*gclwh8_5*x99);
IkReal x307=((0.10915)*gclwh6_5*x96);
IkReal x308=((0.2183)*gclwh6_5*x74);
IkReal x309=((0.10915)*gclwh8_5*x114);
IkReal x310=((0.10915)*gclwh10_5*x70);
IkReal x311=(gclwh12_3*pp*x167);
IkReal x312=(x130*x78);
IkReal x313=(x106*x190);
IkReal x314=((2.0)*gclwh8_11*x99);
IkReal x315=(gclwh10_2*pz*x70);
IkReal x316=(x211*x66);
IkReal x317=(x190*x94);
IkReal x318=(gclwh4_3*pp*x101);
IkReal x319=(x106*x192);
IkReal x320=(x152*x66);
IkReal x321=((2.0)*gclwh6_11*x74);
IkReal x322=(x149*x66);
IkReal x323=(x213*x76);
IkReal x324=(x106*x168);
IkReal x325=(gclwh13_1*pp*x167);
IkReal x326=(x106*x201);
IkReal x327=(x106*x140);
IkReal x328=(gclwh5_1*pp*x101);
IkReal x329=(x182*x66);
IkReal x330=(gclwh11_0*pz*x70);
IkReal x331=(x138*x74);
IkReal x332=(x140*x94);
IkReal x333=(x78*x91);
IkReal x334=(x173*x213);
IkReal x335=(x119*x99);
IkReal x336=(x106*x199);
IkReal x337=(x166*x66);
IkReal x338=(x153*x66);
IkReal x339=((0.10915)*gclwh7_7*x96);
IkReal x340=((0.2183)*gclwh9_7*x99);
IkReal x341=((0.2183)*gclwh11_7*x78);
IkReal x342=((0.10915)*gclwh9_7*x114);
IkReal x343=((0.2183)*gclwh7_7*x74);
IkReal x344=((0.10915)*gclwh11_7*x70);
IkReal x345=((0.10915)*gclwh14_14*x114);
IkReal x346=(x205*x78);
IkReal x347=(x147*x74);
IkReal x348=((0.10915)*gclwh12_14*x96);
IkReal x349=(x148*x99);
IkReal x350=((0.10915)*gclwh4_14*x70);
IkReal x351=((0.09465)*gclwh4_8*x70);
IkReal x352=(x94*x95);
IkReal x353=(gclwh12_3*x304);
IkReal x354=(x207*x66);
IkReal x355=(gclwh3_3*x304);
IkReal x356=(x134*x65);
IkReal x357=(gclwh3_2*r21*x106);
IkReal x358=(x216*x99);
IkReal x359=(x157*x66);
IkReal x360=(x106*x95);
IkReal x361=((0.09465)*pp*x92);
IkReal x362=(x212*x65);
IkReal x363=(x106*x215);
IkReal x364=(x170*x66);
IkReal x365=(x78*x97);
IkReal x366=(gclwh4_2*r22*x106);