function [new_config, new_ctrm, new_norms]  = achilles151_km(sp_coords,Theta)

% This code represents the kinematic model created by 'Build_data_frame' as a function.

%%% INPUTS %%%
% sp_coords: a 4 x 1 vector for global position, R3, and rotation, [-pi,pi], in the spatial frame.
% Theta: a vector of N x 1 angles, where N is the number of rotation axes.
%%% Outputs %%%
% new_config: a point cloud from the surface of the kinematic model in the new configuration.
% new_ctrm: a new center of mass estimate for the kinematic model.

% new_norms: a new norm associated with the model point is calculated to reflect the new pose.

%%% Sampled meshes for each segment
I1  = [  0.000100 -0.007986 -0.008255  0.009863 -0.002744  0.017830  0.016325  0.019153  0.019918  0.026933  0.015715  0.025498  0.014546  0.030399  0.014845  0.020040  0.051156  0.036815  0.036676  0.042981  0.044634  0.054567  0.050633  0.075665  0.080772  0.076580  0.070397  0.067006  0.076430  0.072695  0.085241  0.083184  0.083621  0.073511  0.081868  0.089087  0.086821  0.088432  0.084902  0.106978  0.112588  0.108117  0.109881  0.112424  0.110652  0.105516  0.110738  0.112710  0.115728  0.116870  0.120455  0.123329  0.124618  0.126525  0.107365  0.117722  0.125934  0.123501  0.122698  0.120447  0.111778  0.111243  0.106835  0.097259  0.095841  0.088545  0.088386  0.098063  0.100598  0.098449  0.104833  0.094086  0.091013  0.101083  0.089830  0.090938  0.091352  0.087905  0.127107  0.124744  0.119870  0.122774  0.119939  0.115663  0.112966  0.099731  0.096844  0.101512  0.101503  0.092171  0.118364  0.116374  0.103362  0.064467  0.051076  0.052422  0.054641  0.058495  0.056042  0.060204  0.043841  0.047327  0.040156  0.066707  0.044383  0.044741  0.040418  0.034341  0.069765  0.069846  0.075598  0.076074  0.088386  0.088646  0.062108  0.067678  0.066571  0.068304  0.061175  0.057681  0.027526  0.031807  0.026682  0.024452 -0.000961  0.008517  0.008091 -0.009190
         0.133731  0.119433  0.119542 -0.089760 -0.096042 -0.107976 -0.110722 -0.124134 -0.141563  0.135084  0.134763  0.135184  0.126577  0.125030  0.118544  0.117330 -0.099249 -0.106844 -0.116259 -0.127743 -0.136596 -0.136576 -0.137906 -0.125858 -0.111616 -0.117072 -0.107337 -0.098979 -0.075949 -0.073922 -0.031044  0.069087  0.069506  0.078067  0.076393  0.077023  0.088429  0.094239  0.096922  0.077895  0.065647  0.060112  0.052080  0.032162  0.038474  0.028483  0.010252  0.003931 -0.006392  0.011109  0.013169  0.026609  0.031849  0.028438 -0.066941 -0.043379 -0.030816 -0.019315 -0.004620 -0.034240 -0.030616 -0.023529 -0.017767 -0.042055 -0.006140 -0.047825 -0.053794 -0.048561 -0.057729 -0.065963 -0.068131 -0.089813 -0.105372 -0.092283 -0.084120 -0.070876 -0.064958 -0.065525 -0.013446  0.054861  0.055129  0.039025  0.030306  0.021801 -0.013323  0.029197  0.045828  0.051861  0.050375  0.061472  0.043379  0.058752  0.080881  0.121262  0.126043  0.123232  0.119295  0.096693  0.091903  0.089454  0.102472  0.100519  0.113964  0.096124  0.129912  0.131178  0.133446  0.132992  0.110738  0.110691  0.105300  0.088044  0.065873 -0.030865 -0.092270 -0.106417 -0.121927 -0.131577 -0.134943 -0.112369 -0.103851  0.135096 -0.139944 -0.110753 -0.093367  0.120042  0.134414 -0.094347
         0.045501 -0.017294 -0.052194 -0.052926 -0.029248 -0.016033  0.014379  0.040340  0.095020  0.085597  0.045804  0.034306  0.006862 -0.006433 -0.049480 -0.056818 -0.047756 -0.010330  0.011814  0.032100  0.059365  0.074950  0.104097  0.068756  0.050033  0.043427  0.004292 -0.018832 -0.035463 -0.051800 -0.062556 -0.051399 -0.042425 -0.016194  0.002315  0.020654  0.035281  0.057659  0.087430  0.074258  0.057391  0.036839  0.027660  0.005457 -0.009485 -0.045584 -0.034901 -0.027293 -0.003095  0.013693  0.034481  0.038811  0.060133  0.087900  0.073238  0.066720  0.086009  0.061966  0.047899  0.063166  0.014171 -0.000350 -0.025734 -0.019656 -0.054027 -0.040585 -0.031181 -0.005407  0.020677  0.029435  0.064436  0.058763  0.077176  0.091037  0.039158  0.008304 -0.000851 -0.012305  0.085018  0.087340  0.060654  0.050618  0.028568  0.012052 -0.009738 -0.057290 -0.055804 -0.034876 -0.006685 -0.000208  0.036718  0.055196  0.086116  0.098958  0.060311  0.017279  0.013235 -0.017834 -0.038908 -0.053484 -0.066422 -0.040963 -0.024963  0.000754  0.015174  0.032104  0.064670 -0.001326  0.041239  0.076199  0.061597  0.008935 -0.038814 -0.057804 -0.038525 -0.004034  0.030699  0.082652  0.094290 -0.003602 -0.038326  0.052086  0.079015  0.009740 -0.059905 -0.016986  0.045746 -0.048629
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N1  = [ -0.132069 -0.049193 -0.040023  0.034966 -0.509833  0.083622 -0.049056 -0.235340  0.111539  0.004146 -0.026328  0.013613 -0.430128 -0.129772  0.204905  0.337592  0.548524  0.071321 -0.233094 -0.186542  0.114491  0.269025  0.311004  0.814207  0.969287  0.891456  0.765323  0.629029  0.941551  0.933745  0.866720  0.812375  0.728093  0.810699  0.844483  0.852583  0.849960  0.780055  0.789197  0.797958  0.907764  0.889343  0.905860  1.049363  1.000223  0.992913  1.035157  1.054500  1.094297  1.113622  1.041708  1.109230  1.119198  1.119703  0.945444  0.954536  1.080975  1.105053  1.080435  1.027571  1.025325  1.019846  0.986736  0.968893  0.944371  0.972768  0.961800  0.970937  0.970295  0.954309  1.061607  0.913706  0.923574  0.926449  0.975806  0.966577  0.964043  0.960625  1.081712  1.098197  1.002536  1.089060  1.000430  1.098994  1.044332  1.007427  0.967225  0.888826  0.855136  0.848655  0.992508  0.941835  0.733774  0.625901  0.659484  0.755240  0.791070  0.817886  0.743546  0.671054  0.637858  0.745782  0.585206  0.830124  0.621147  0.543332  0.395665 -0.051537  0.763978  0.775167  0.720303  0.839438  0.742506  0.933950  0.671633  0.675932  0.663454  0.577131  0.455007  0.329796 -0.140672  0.129430  0.057947 -0.225017  0.167156 -0.020630 -0.056929  0.193759
         1.121896  1.118583  1.119029 -1.088873 -0.723108 -1.101762 -1.069626 -1.003099 -1.135906  1.134822  1.133878  1.135095  0.908564  0.857454  1.100057  1.065275 -0.963770 -1.086935 -0.984307 -0.985061 -1.115611 -1.112137 -1.103153 -0.797636 -0.531472 -0.643236 -0.761000 -0.820909 -0.510532 -0.515642 -0.239886  0.738734  0.834057  0.722283  0.640297  0.638155  0.655124  0.813987  0.798554  0.797780  0.602843  0.595242  0.564191  0.376308  0.481348  0.110762 -0.065364 -0.138732 -0.171754  0.081672 -0.119370  0.109370  0.089687  0.063462 -0.427611 -0.467947 -0.174755 -0.144430 -0.133022 -0.348785 -0.383971 -0.384104 -0.389167 -0.461179 -0.192944 -0.441220 -0.474044 -0.472547 -0.499861 -0.540459 -0.338293 -0.552357 -0.634526 -0.607264 -0.532736 -0.497325 -0.490748 -0.489823 -0.061451  0.276568  0.467651  0.234457  0.088898  0.188852 -0.309823  0.052023  0.395226  0.654114  0.705431  0.676439  0.454423  0.552675  0.856776  0.948534  0.918089  0.799350  0.736522  0.667540  0.815690  0.879399  0.904787  0.801167  0.912599  0.657381  0.933872  0.998016  1.065225  0.967767  0.826276  0.810333  0.868523  0.649312  0.821676 -0.262385 -0.852275 -0.831695 -0.874184 -0.992401 -1.053269 -1.005628 -1.033044  1.130246 -1.131964 -1.013694 -1.078243  1.119130  1.132215 -1.070427
        -0.032351 -0.018088 -0.056269 -0.086726 -0.620558 -0.105810 -0.261717 -0.362963  0.041288  0.083284  0.047173  0.028183 -0.429900 -0.668173 -0.072016 -0.080482 -0.120051 -0.205862 -0.404978 -0.428686 -0.132074  0.027109  0.126545  0.011563 -0.135068 -0.199739 -0.295378 -0.422500 -0.285870 -0.303747 -0.650498 -0.192307 -0.053314 -0.220006 -0.314591 -0.299041 -0.275325 -0.002533 -0.020646  0.008589 -0.223883 -0.284601 -0.295079 -0.055430  0.102433 -0.499188 -0.408710 -0.331724 -0.125801 -0.025106 -0.331207 -0.106620 -0.026265 -0.023323 -0.336078 -0.278945 -0.173179 -0.082606 -0.209496 -0.216455 -0.187252 -0.211127 -0.322105 -0.273803 -0.549103 -0.292359 -0.277227 -0.246913 -0.198744 -0.176372 -0.043250 -0.279285 -0.086647 -0.140404 -0.078274 -0.218404 -0.239833 -0.253823 -0.208966  0.030459 -0.164570 -0.117006 -0.441861 -0.059745 -0.221033 -0.476297 -0.402729 -0.166889  0.047517 -0.222801 -0.221963 -0.218032  0.109944  0.078662  0.010289 -0.203878 -0.263717 -0.330018 -0.097815 -0.000112 -0.007820 -0.186747 -0.280114 -0.318911 -0.129706  0.031900 -0.010081 -0.545179 -0.036700 -0.037924  0.104563 -0.310837 -0.008999 -0.539322 -0.264072 -0.326524 -0.248300  0.091424  0.133952 -0.361432 -0.367429  0.064159 -0.043131 -0.340207 -0.018011 -0.048175  0.032894  0.029338
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I2  = [ -0.008851 -0.000928 -0.002374 -0.003283 -0.004691  0.015752  0.003750  0.003555  0.005071  0.022984  0.025227  0.026637  0.022169  0.020030  0.045276  0.051230  0.055696  0.049984  0.041033  0.069389  0.070222  0.072608  0.074470  0.062545  0.065156  0.056872  0.055447  0.063449  0.057863  0.058634  0.059125  0.061815  0.063716  0.064471  0.062833  0.042150  0.032085  0.037195  0.047366  0.050277  0.062378  0.049649  0.064179  0.053731  0.066915  0.058067  0.034467  0.033744  0.037318  0.037952  0.035290  0.036426  0.020852  0.028998  0.055306  0.044002  0.007718  0.012439  0.011896 -0.008209 -0.008416 -0.003093  0.006088 -0.009116
         0.119861  0.120216  0.123468  0.123716  0.123761 -0.077007 -0.088562 -0.089246 -0.089942  0.114013  0.110897  0.102212  0.104431  0.097840 -0.023040 -0.017702 -0.014727 -0.032628 -0.058647 -0.000677  0.012783  0.020014  0.031062  0.064406  0.069425  0.073711  0.067712  0.046325  0.053043  0.039082  0.014361  0.014598  0.014841  0.021691  0.032141  0.075000  0.089148  0.090653  0.085439  0.085240  0.038882  0.069837  0.057655  0.082080  0.024172 -0.023513 -0.072329 -0.064975 -0.046027 -0.037768 -0.042457 -0.037662 -0.074636 -0.067276 -0.029163 -0.031030  0.113745  0.112681  0.116227 -0.091915 -0.092284 -0.098026  0.119280 -0.101832
        -0.104428 -0.121057 -0.139786 -0.157087 -0.164822 -0.172454 -0.149674 -0.132363 -0.115742 -0.112841 -0.117763 -0.136259 -0.147417 -0.180286 -0.177978 -0.162410 -0.146536 -0.135397 -0.122690 -0.117564 -0.126499 -0.122406 -0.115698 -0.126311 -0.111643 -0.130008 -0.145155 -0.143793 -0.165214 -0.184229 -0.186196 -0.170277 -0.153778 -0.151812 -0.170826 -0.167562 -0.165963 -0.142250 -0.127921 -0.112947 -0.168187 -0.157555 -0.129439 -0.108665 -0.137791 -0.122248 -0.117667 -0.133953 -0.157373 -0.172218 -0.173210 -0.180595 -0.156885 -0.144674 -0.122681 -0.161986 -0.176473 -0.155444 -0.127780 -0.125732 -0.134140 -0.178294 -0.112002 -0.167284
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N2  = [  0.011667  0.241144  0.438530  0.457733  0.402536  0.832146  0.516320  0.295784  0.274411  0.553219  0.658255  0.711186  0.713111  0.779424  0.888063  0.922747  0.949434  0.924806  0.889020  0.954387  0.990022  1.015246  1.022123  0.938824  0.846245  0.871461  0.897574  1.015604  0.954995  0.982731  1.029473  1.040343  1.049583  1.059027  1.049906  0.805980  0.789845  0.730951  0.706001  0.767214  1.037835  0.847540  0.971139  0.775740  1.024648  0.942309  0.739278  0.796015  0.903230  0.907799  0.906148  0.912490  0.811698  0.843355  0.924750  0.896087  0.662009  0.667667  0.446400  0.157064  0.258199  0.527730  0.117016  0.331608
         1.119034  1.083115  1.015521  1.007618  1.026660 -0.648923 -0.946073 -1.043147 -1.049964  0.939197  0.840834  0.806414  0.819129  0.672891 -0.530652 -0.487717 -0.416749 -0.456138 -0.507365 -0.346510 -0.157451 -0.051459  0.117272  0.430663  0.653781  0.571047  0.547371  0.249111  0.478142  0.186383 -0.055403 -0.142027 -0.114508 -0.015408  0.107664  0.660319  0.680060  0.793712  0.804995  0.773884  0.231026  0.603340  0.363854  0.762253 -0.005553 -0.415722 -0.709740 -0.644410 -0.490441 -0.497236 -0.499592 -0.507375 -0.672166 -0.617027 -0.441770 -0.508726  0.851163  0.863323  1.009694 -1.078131 -1.051197 -0.941999  1.110955 -1.041903
        -0.069316 -0.001780 -0.040565 -0.235701 -0.302443 -0.252528 -0.105553 -0.200734 -0.191992 -0.307579 -0.375575 -0.324655 -0.256069 -0.484650 -0.356977 -0.302213 -0.345559 -0.370611 -0.404773 -0.429297 -0.480037 -0.448483 -0.423139 -0.439345 -0.331707 -0.428508 -0.391631 -0.372437 -0.285402 -0.536857 -0.417619 -0.304253 -0.260249 -0.249191 -0.312183 -0.439529 -0.442771 -0.298491 -0.348010 -0.221443 -0.275722 -0.438170 -0.418687 -0.235425 -0.423909 -0.375802 -0.429033 -0.422395 -0.386924 -0.351818 -0.353863 -0.289593 -0.289251 -0.330677 -0.394382 -0.375892 -0.344154 -0.240371 -0.241460 -0.133568 -0.037173 -0.255337 -0.177404 -0.154093
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I3  = [ -0.141563 -0.130520 -0.123859 -0.132424 -0.139531 -0.121384 -0.130443 -0.135671 -0.129584 -0.086255 -0.096434 -0.102073 -0.086444 -0.089844 -0.097918 -0.106159 -0.108881 -0.094672 -0.106916 -0.090767 -0.105235 -0.088402 -0.050242 -0.034687 -0.041820 -0.047744 -0.040858 -0.031866 -0.035799 -0.056982 -0.050618 -0.034484 -0.036532 -0.048997 -0.032315 -0.020472 -0.025763 -0.024813 -0.016212 -0.017909 -0.034600 -0.018279 -0.003976 -0.002134 -0.012639 -0.007746  0.001848 -0.017742 -0.012080 -0.008920 -0.013213 -0.004273 -0.004893  0.001244  0.006583  0.003376  0.002765 -0.005139 -0.016745 -0.018823  0.000442 -0.008632  0.007026  0.015037  0.007060  0.019367  0.003961  0.007703  0.020683  0.003215  0.007610  0.003174  0.018990  0.017866  0.017962  0.015358  0.018132  0.018435  0.023558  0.036055  0.039439  0.029052  0.025344  0.035618  0.035164  0.019043  0.023942  0.012136  0.007612  0.027448  0.019766  0.035518  0.037985  0.039205  0.049582  0.056791  0.034474  0.038280  0.038217  0.041708  0.050895  0.039979  0.045213  0.042088  0.039323  0.056265  0.072786  0.072264  0.069814  0.044052  0.041141  0.043031  0.065394  0.060850  0.053107  0.069275  0.061757  0.053674  0.053842  0.069050  0.070257  0.081947  0.077821  0.088625  0.072809  0.068571  0.072488  0.086433  0.089420  0.077548  0.073368  0.067668  0.081224  0.070210  0.099207  0.108709  0.106942  0.089036  0.105504  0.097812  0.108713  0.093247  0.089062  0.085461  0.112868  0.108043  0.103100  0.107395  0.112385  0.125774  0.105833  0.112940  0.114830  0.123112  0.132927  0.142250  0.130449  0.124118  0.142123  0.135717  0.129275  0.141784  0.150171  0.148564  0.133366  0.124928  0.111215  0.121673  0.114481  0.096177  0.095677  0.095912  0.106474  0.114830  0.077922  0.114597  0.097595  0.053123  0.095189  0.062415  0.055871  0.063477  0.056373  0.079248  0.071621  0.080292  0.063971  0.030648  0.058414  0.027433  0.024064  0.028438  0.024386  0.025435  0.033093  0.009327  0.012000  0.016512  0.003786  0.041488  0.012440  0.007053  0.021355  0.044514  0.009367  0.006490  0.008298 -0.000944 -0.001178  0.019632 -0.016270 -0.003427  0.020051  0.016427 -0.021912 -0.022646 -0.015771 -0.003988 -0.005118  0.003306  0.007165  0.001521 -0.052380 -0.061488 -0.069701 -0.080456 -0.072986 -0.012169 -0.020579 -0.068677 -0.061199 -0.074544 -0.063351 -0.074916 -0.059347 -0.052662 -0.070811 -0.071937 -0.026035 -0.056586 -0.025804 -0.081465 -0.082943 -0.087509 -0.120045 -0.076951 -0.080466 -0.156404 -0.153475 -0.155921 -0.147667 -0.150243 -0.112059 -0.115064 -0.116618 -0.147882
        -0.024328 -0.021064 -0.011096  0.007632  0.019696  0.026675  0.033629  0.036713  0.049247  0.066920  0.057474  0.046294  0.045327  0.035162  0.026641  0.019110 -0.003521  0.000245 -0.017655 -0.016609 -0.035301 -0.031795  0.100508  0.104577  0.084990  0.094805  0.080447  0.066627  0.058456  0.050639  0.029557  0.033954  0.025778  0.005915  0.001182 -0.020440 -0.021935 -0.028077 -0.036356 -0.039728 -0.052901 -0.047679  0.102450  0.102760  0.102153  0.089869  0.091365  0.077324  0.082887  0.073874  0.050769  0.062672  0.053670  0.065551  0.054693  0.039196  0.016051  0.032193  0.022235  0.005101 -0.021110 -0.012903 -0.028629 -0.036919 -0.039889 -0.049200 -0.045082 -0.047320 -0.057319 -0.043385 -0.049639 -0.053174 -0.056568 -0.056798  0.089198  0.091349  0.094951  0.100704  0.103143  0.108019  0.106119  0.099192  0.082877  0.090815  0.082365  0.066782  0.073755  0.024109  0.032092 -0.044450 -0.047018 -0.062665 -0.062376 -0.061033 -0.070816 -0.075522 -0.067293 -0.071415 -0.070870 -0.065870 -0.062787 -0.054717 -0.042168 -0.037704 -0.032250 -0.023604  0.021737  0.046895  0.057884  0.073849  0.081129  0.085988  0.074643  0.097775  0.108871  0.106172  0.109517  0.107681  0.104969 -0.081460 -0.086183 -0.089536 -0.086954 -0.080019 -0.073115 -0.062221 -0.058374 -0.050403 -0.041175 -0.033764 -0.022840 -0.011048 -0.004565  0.004960 -0.004347 -0.005423  0.021056  0.049400  0.057459  0.082117  0.089395  0.102146  0.104241  0.106927 -0.081631 -0.072835 -0.062786 -0.058733 -0.046971 -0.040961 -0.034339 -0.026794 -0.017709 -0.006786 -0.019335 -0.003378  0.030877  0.013581  0.049052  0.064185  0.082404 -0.032586 -0.020736  0.001356 -0.043600 -0.054593  0.098795  0.084367  0.030442 -0.052520 -0.066093 -0.091699  0.101120  0.092872  0.067245  0.006375 -0.024772 -0.079354 -0.083741 -0.083611 -0.081092 -0.079987 -0.076028  0.107994  0.099648  0.029867 -0.006676 -0.046488 -0.036662 -0.057280 -0.059684 -0.063573 -0.059897 -0.060484 -0.056652 -0.015270  0.062443  0.072731  0.070048  0.094713  0.087833  0.090758  0.096035  0.109123  0.103073  0.099320  0.096769 -0.056116 -0.041601 -0.056119 -0.029544 -0.035706 -0.056188 -0.042356  0.018468  0.031794  0.041095  0.002069  0.038641  0.079761  0.095244  0.101026 -0.047475 -0.032507 -0.011767 -0.042780 -0.051688 -0.039583 -0.025593  0.005488  0.024201  0.017041  0.039337  0.050936  0.060753  0.066907  0.071728  0.076598  0.097381  0.096986  0.105353 -0.032731  0.004068  0.029866  0.049553  0.061204  0.068104 -0.004108 -0.004346  0.005502  0.028585  0.007821  0.042568  0.022154 -0.013044 -0.023273
        -0.373207 -0.378805 -0.388006 -0.398016 -0.393145 -0.394297 -0.384942 -0.372505 -0.362492 -0.356221 -0.361857 -0.379613 -0.377690 -0.384546 -0.390654 -0.394711 -0.390629 -0.387903 -0.378523 -0.374785 -0.361664 -0.361355 -0.327952 -0.324512 -0.345333 -0.334702 -0.350245 -0.364959 -0.371238 -0.372800 -0.379947 -0.385183 -0.385253 -0.383593 -0.393724 -0.398922 -0.378796 -0.362021 -0.349697 -0.337014 -0.327661 -0.328890 -0.312090 -0.330694 -0.338004 -0.354412 -0.362780 -0.363659 -0.358089 -0.379456 -0.406943 -0.406758 -0.422328 -0.414868 -0.442756 -0.449681 -0.453620 -0.433985 -0.412920 -0.412544 -0.441194 -0.430001 -0.442683 -0.445130 -0.421475 -0.413687 -0.391863 -0.378608 -0.381048 -0.357557 -0.341297 -0.325897 -0.313990 -0.301582 -0.297822 -0.306681 -0.327644 -0.341613 -0.372121 -0.370640 -0.389392 -0.396128 -0.423295 -0.422584 -0.439383 -0.442751 -0.438620 -0.468609 -0.460045 -0.455401 -0.433592 -0.442944 -0.418129 -0.408880 -0.402279 -0.398938 -0.369669 -0.356240 -0.338949 -0.324358 -0.319986 -0.314009 -0.305429 -0.301050 -0.290985 -0.292412 -0.280500 -0.291598 -0.297272 -0.299858 -0.318159 -0.326151 -0.313098 -0.346633 -0.370388 -0.366178 -0.379511 -0.396858 -0.412922 -0.388706 -0.368189 -0.364306 -0.352189 -0.346522 -0.331528 -0.320606 -0.318032 -0.316156 -0.308013 -0.299969 -0.292799 -0.283243 -0.282225 -0.275905 -0.286731 -0.290795 -0.294685 -0.298975 -0.309806 -0.328294 -0.338194 -0.353167 -0.360496 -0.376002 -0.361555 -0.349560 -0.336667 -0.334619 -0.325669 -0.327540 -0.308450 -0.304502 -0.298693 -0.298446 -0.310433 -0.311098 -0.310460 -0.300497 -0.326985 -0.328812 -0.344785 -0.326685 -0.321534 -0.315447 -0.334587 -0.339726 -0.361833 -0.335848 -0.301738 -0.322643 -0.336281 -0.373726 -0.371536 -0.346669 -0.308085 -0.294007 -0.296137 -0.339143 -0.354248 -0.343959 -0.365306 -0.410992 -0.426126 -0.396468 -0.351488 -0.287724 -0.278756 -0.297715 -0.302046 -0.319065 -0.332925 -0.346215 -0.359168 -0.375594 -0.409588 -0.455004 -0.440311 -0.430727 -0.407041 -0.423277 -0.388153 -0.370544 -0.387915 -0.370829 -0.351070 -0.326449 -0.299440 -0.319246 -0.352138 -0.354025 -0.379959 -0.411115 -0.371680 -0.438736 -0.404518 -0.399395 -0.408334 -0.441886 -0.431410 -0.388193 -0.363648 -0.301710 -0.337457 -0.352886 -0.372274 -0.349706 -0.340017 -0.339782 -0.384147 -0.381688 -0.381375 -0.384582 -0.377011 -0.372620 -0.364957 -0.355439 -0.351030 -0.343057 -0.339497 -0.327681 -0.316276 -0.358543 -0.385243 -0.387024 -0.373833 -0.364660 -0.356994 -0.387775 -0.390562 -0.382064 -0.369150 -0.390513 -0.381385 -0.395767 -0.384832 -0.374383
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N3  = [ -0.260013 -0.079241 -0.010245 -0.263768 -0.615389 -0.084104 -0.340938 -0.651333 -0.654477 -0.377151 -0.316939 -0.051490  0.068325  0.159728  0.084291  0.134555  0.105806  0.213001  0.050607  0.162155 -0.023377  0.113333 -0.408612 -0.089421 -0.185154 -0.362883 -0.210203 -0.508123 -0.400943 -0.125193 -0.168339 -0.616792 -0.553974 -0.351832 -0.746269 -0.850530 -0.661750 -0.387429 -0.221387  0.183942  0.163277 -0.071769  0.305341  0.170370 -0.159297 -0.523490 -0.583330 -0.416032 -0.452258 -0.583854 -0.843234 -0.671442 -0.835513 -0.639778 -0.709445 -0.856513 -0.854817 -0.909086 -0.893370 -0.892296 -0.760306 -0.851510 -0.682305 -0.696856 -0.562411 -0.450990 -0.574718 -0.599176 -0.539320 -0.560265 -0.398354 -0.009635  0.159374  0.696983  0.626340  0.586503  0.363477  0.222063 -0.448181 -0.144873 -0.253620 -0.515561 -0.537430 -0.487725 -0.459671 -0.507351 -0.528992 -0.815987 -0.831366 -0.696594 -0.674667 -0.485191 -0.628198 -0.635947 -0.555114 -0.422790 -0.537920 -0.570702 -0.537461 -0.235087  0.015451  0.083403  0.215153  0.342536  0.678584  0.194211  0.573860  0.525791  0.434679  0.431825  0.183459  0.160051  0.240857  0.169185  0.089352  0.157685  0.098728 -0.055811 -0.106471 -0.248330 -0.228718 -0.104374  0.132218  0.416374  0.379141  0.204268  0.243342  0.349311  0.312338  0.127453  0.087690  0.309769  0.250913  0.550087  0.455029  0.508976  0.499550  0.381506  0.425485  0.206500  0.286009  0.086204  0.065412  0.202443  0.454879  0.473789  0.440847  0.452423  0.484330  0.507644  0.435248  0.473251  0.524203  0.617152  0.600573  0.723748  0.620898  0.600746  0.745248  0.719446  0.774081  0.462398  0.607425  0.738224  0.510159  0.480024  0.688370  0.520640  0.529377  0.432504  0.426486  0.049579  0.643445  0.546948  0.281889  0.556966  0.321202 -0.297782  0.400386 -0.113159 -0.357491 -0.066266 -0.250644  0.155062  0.067272  0.423021  0.491470  0.840312  0.090732 -0.119893 -0.498369 -0.598195 -0.610158 -0.543873 -0.462750 -0.696650 -0.602233 -0.550592 -0.689063 -0.477883 -0.626565 -0.616741 -0.588844 -0.009255 -0.241257  0.363805  0.566570  0.200985 -0.502098 -0.595857 -0.629227 -0.654601 -0.595752 -0.701522 -0.880865 -0.858301 -0.877516 -0.865309 -0.899653 -0.615731 -0.565267  0.447469  0.143821  0.118764  0.049600  0.134027  0.134312 -0.413963 -0.690995  0.028068  0.018651  0.191344 -0.017330 -0.064549  0.016208 -0.013035 -0.097997 -0.320855 -0.221687 -0.462368 -0.010350  0.143888  0.296155  0.238999 -0.527286 -0.228447 -0.222069 -0.936988 -0.607028 -1.049488 -0.823644 -0.783001 -0.310481  0.042566  0.039195 -0.373281
        -0.939153 -0.803721 -0.649987 -0.127932  0.362149  0.559588  0.940887  0.783355  0.847839  0.803185  0.909494  0.692180  0.609763  0.529264  0.354965  0.211235 -0.528109 -0.416031 -0.675831 -0.618502 -0.757841 -0.663600  0.849370  1.090435  0.772184  0.721131  0.756019  0.683612  0.797740  0.528833  0.239966  0.184637  0.151932 -0.080519 -0.099287 -0.510070 -0.763884 -0.877509 -0.957018 -0.948150 -0.720413 -0.702321  1.053404  1.085533  0.994890  0.679306  0.740815  0.857970  0.848294  0.787588  0.454262  0.770134  0.412699  0.810954  0.639481  0.257976  0.007079  0.180758  0.166879  0.030768 -0.404070 -0.260104 -0.566277 -0.586500 -0.813999 -0.918070 -0.856595 -0.836910 -0.885809 -0.859686 -0.943382 -1.005164 -1.043407 -0.785358  0.880372  0.910213  0.979780  0.991160  0.931529  1.089893  1.048048  0.905275  0.821106  0.800496  0.872411  0.732303  0.760334  0.024256  0.193520 -0.382004 -0.756010 -0.904233 -0.790667 -0.796956 -0.865783 -0.952432 -0.880750 -0.863848 -0.879560 -0.807947 -0.666070 -0.768041 -0.633391 -0.751592 -0.556458 -0.669424  0.363714  0.455649  0.638588  0.950435  0.982307  0.905712  0.862125  0.946193  1.097567  1.064144  1.108239  1.091167  1.074172 -1.015151 -1.025183 -1.070721 -1.011225 -0.781220 -0.696597 -0.718560 -0.667498 -0.695099 -0.720547 -0.603740 -0.593186 -0.615962 -0.302006 -0.198675 -0.160282 -0.155620  0.323228  0.489642  0.532107  0.815198  0.876223  1.003461  1.078801  1.099913 -0.826473 -0.718498 -0.752002 -0.729228 -0.696560 -0.708868 -0.712060 -0.654737 -0.480113 -0.202116 -0.467722 -0.099980  0.353958  0.262575  0.425920  0.518778  0.816912 -0.746567 -0.413876  0.018531 -0.659731 -0.703060  0.911402  0.810161  0.360624 -0.707855 -0.766398 -1.071418  0.939273  0.907621  0.740344  0.157523 -0.626460 -0.982955 -0.822345 -1.031085 -0.977940 -1.065325 -0.947702  1.104278  0.993373  0.391005 -0.460631 -0.518848 -0.653973 -0.993933 -0.906239 -0.842882 -0.832605 -0.882318 -0.924537 -0.305368  0.681614  0.818738  0.711219  0.793293  0.811448  0.761325  0.843164  1.105985  1.053105  1.017638  0.923543 -0.933177 -0.881115 -0.842679 -0.818120 -0.750166 -0.843875 -0.687071  0.177806  0.168196  0.247010 -0.047539  0.232243  0.811827  0.783635  0.994218 -0.691255 -0.690686 -0.598376 -0.681947 -0.686124 -0.875316 -0.756810 -0.285174  0.253670  0.019290  0.356238  0.607321  0.790813  0.769537  0.940409  0.870957  0.720309  0.787046  1.104468 -0.657859 -0.321370  0.448003  0.673845  0.663243  0.888887 -0.273246 -0.413222  0.276010  0.730347  0.383727  0.529589  0.363362 -0.665529 -0.925801
        -0.759297 -0.999141 -1.148868 -1.380039 -1.203260 -1.239645 -0.749050 -0.792764 -0.657005 -0.967198 -0.836661 -1.141370 -1.188528 -1.217358 -1.317478 -1.346102 -1.214472 -1.243503 -1.114723 -1.132251 -1.048130 -1.109770 -0.885426 -0.482909 -1.057527 -1.047726 -1.067827 -0.991464 -0.937039 -1.248402 -1.350446 -1.184065 -1.231621 -1.332708 -1.086671 -0.665879 -0.591006 -0.745392 -0.681789 -0.703108 -1.045485 -1.082934 -0.315223 -0.264370 -0.764046 -0.976161 -0.848353 -0.845279 -0.827552 -0.779534 -0.791987 -0.639960 -0.847967 -0.597800 -0.823975 -0.910905 -0.967888 -0.834996 -0.871840 -0.898738 -0.965220 -0.907963 -0.928230 -0.882358 -0.697983 -0.568051 -0.472956 -0.287808 -0.380559 -0.484619 -0.532128 -0.631759 -0.394248 -0.391028 -0.360437 -0.249641 -0.014890  0.065355 -0.674180 -0.427103 -0.553365 -0.627705 -0.795187 -0.894246 -0.801273 -0.971893 -0.910713 -1.029155 -0.979716 -1.056916 -0.556448 -0.586560 -0.257530 -0.358010 -0.353589 -0.366826 -0.472879 -0.390755 -0.218057  0.286139  0.476753  0.385479  0.482971  0.331480  0.271644  0.458512  0.514470  0.500383  0.430503 -0.014849  0.091262  0.234526  0.277736  0.171489 -0.224904 -0.093279 -0.345042 -0.540967 -0.599859 -0.554515 -0.538168 -0.313718  0.025652  0.286643  0.387798  0.421556  0.456420  0.401662  0.391102  0.520175  0.528480  0.475351  0.657315  0.577471  0.634722  0.613211  0.573964  0.549936  0.510146  0.343106  0.252964  0.079939 -0.137618 -0.358847  0.211367  0.320775  0.304354  0.322187  0.337423  0.311268  0.348955  0.385326  0.487817  0.548767  0.451313  0.496693  0.498910  0.542609  0.376015  0.343948 -0.133306  0.295760  0.476188  0.492022  0.357085  0.333622 -0.442839  0.224552  0.546108  0.353686  0.296284 -0.568672 -0.467247  0.039930  0.402784  0.589998  0.470655 -0.093440  0.246851 -0.076626 -0.522767 -0.300193 -0.808129 -0.437347  0.097106  0.579523  0.503017  0.050595  0.484009 -0.001299 -0.230897 -0.343142 -0.375807 -0.397452 -0.439858 -1.101099 -0.929540 -0.779820 -0.736980 -0.915455 -0.649025 -0.772069 -0.651458 -0.312746 -0.537155 -0.156105 -0.368554 -0.755123 -0.562605 -0.403983 -0.429264 -0.667082 -0.353345 -0.701208 -0.891150 -0.931443 -0.872009 -0.947518 -0.834304 -0.672575 -0.809115 -0.359495 -1.077087 -1.083851 -1.173309 -1.088260 -1.084676 -0.714097 -0.510103 -1.333610 -1.351410 -1.348583 -1.324352 -1.203479 -1.044151 -1.065890 -0.845656 -0.897164 -1.096915 -0.926978 -0.355386 -1.105826 -1.251484 -1.234700 -1.040477 -1.148623 -0.910405 -0.951914 -1.182465 -0.740343 -0.594058 -1.067500 -1.231937 -1.322444 -1.126443 -0.741313
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I4  = [  0.003872  0.019980  0.039358  0.026374  0.032159  0.034891  0.045243  0.044290  0.033680  0.054899  0.092652  0.072207  0.078639  0.076981  0.083737  0.090725  0.074797  0.110267  0.115035  0.111368  0.108476  0.133349  0.136153  0.137021  0.131285  0.140045  0.142382  0.144433  0.139953  0.132010  0.129274  0.136572  0.120432  0.109395  0.110128  0.116409  0.094800  0.095290  0.102572  0.120400  0.102221  0.138848  0.141485  0.146471  0.148237  0.140356  0.139779  0.136431  0.129482  0.149126  0.139621  0.096183  0.090528  0.098333  0.056310  0.059302  0.058995  0.091314  0.069003  0.061675  0.043312  0.045844  0.021067  0.007679
         0.115634 -0.141962  0.095696  0.107480  0.118380  0.124361 -0.142404 -0.142991 -0.137501 -0.132458 -0.124512 -0.139564 -0.137217  0.110506  0.103497  0.091754  0.086022  0.065630  0.074604  0.081943  0.081685  0.050918  0.049482  0.039398  0.006591  0.005087  0.022468  0.032411 -0.028065 -0.065524 -0.074889 -0.058733 -0.080579 -0.107976 -0.103160 -0.093991 -0.125362 -0.125715 -0.116656 -0.089114 -0.113009 -0.033584 -0.025642 -0.024367 -0.013578 -0.005951 -0.043650 -0.052597 -0.055099  0.004233  0.022105  0.091904  0.094606  0.086125  0.095726  0.105424  0.120100 -0.130113 -0.128731 -0.141293  0.122048  0.096618 -0.144860  0.104390
         0.230322  0.217157  0.256507  0.233866  0.195817  0.155823  0.167722  0.203005  0.243452  0.265257  0.235611  0.216153  0.182566  0.145834  0.176506  0.206024  0.254001  0.251556  0.215323  0.180650  0.144437  0.177781  0.212539  0.231451  0.257088  0.201304  0.180031  0.163235  0.151233  0.164804  0.208547  0.230286  0.258539  0.226369  0.186464  0.180355  0.145546  0.189623  0.224968  0.189060  0.239602  0.243304  0.210761  0.174446  0.165172  0.224688  0.226626  0.241347  0.146260  0.164365  0.218413  0.153143  0.195574  0.211582  0.238820  0.209780  0.158710  0.153569  0.264609  0.184786  0.157022  0.247159  0.169907  0.262064
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N4  = [  0.222228 -0.002905  0.361584  0.279716  0.214687  0.294375  0.143665  0.097510  0.020775  0.205469  0.760713  0.300066  0.407748  0.725054  0.585057  0.567589  0.400876  0.767495  0.701476  0.804853  0.736042  1.025178  1.081474  1.083625  0.957926  1.133195  1.069956  1.114232  1.003043  1.006139  0.958426  1.097224  0.905090  0.931742  0.915174  0.927489  0.812618  0.859733  0.862799  0.953570  0.811090  1.106876  1.129736  1.125181  1.124280  1.140215  1.129257  1.099541  0.999078  1.144776  1.138777  0.801278  0.584809  0.588867  0.372334  0.337916  0.263101  0.713124  0.485283  0.207464  0.218873  0.371240  0.113561  0.255252
         1.045258 -1.125958  0.989528  1.037643  1.078426  1.053110 -1.136789 -1.140789 -1.120816 -1.092583 -0.816122 -1.111897 -1.079619  0.848829  0.943322  0.940836  0.971484  0.711141  0.871966  0.798776  0.857090  0.498334  0.372466  0.287333  0.224731  0.087215  0.223007  0.255267 -0.358670 -0.478611 -0.627068 -0.284886 -0.622467 -0.668131 -0.692223 -0.676908 -0.809053 -0.768935 -0.761150 -0.641872 -0.774707  0.032497  0.019815 -0.224542 -0.178833 -0.016099 -0.161699 -0.266368 -0.423709  0.038632  0.059688  0.794028  0.933096  0.940511  0.993977  1.039103  1.097636 -0.911177 -0.983765 -1.130258  1.093723  0.988389 -1.140094  1.032229
         0.527171  0.393874  0.568331  0.499600  0.407937  0.420573  0.128832  0.242584  0.424908  0.500816  0.510149  0.267633  0.122860 -0.040928  0.384769  0.433318  0.585104  0.640615  0.357805  0.108331  0.214486  0.244556  0.257802  0.437532  0.775817  0.284421  0.495292  0.262353 -0.230574 -0.090649  0.121342  0.391543  0.559651  0.326223  0.116418  0.131811  0.013999  0.233162  0.306716  0.172101  0.483860  0.485287  0.356686  0.129098  0.023640  0.211312  0.310280  0.404815 -0.182266  0.077769  0.201849  0.053832  0.424965  0.383047  0.544229  0.434763  0.211276  0.096192  0.573847  0.158441  0.315216  0.561581  0.139007  0.541029
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I5  = [ -0.065679 -0.067360 -0.053844 -0.058688 -0.052992 -0.051125 -0.048786 -0.043180 -0.044312 -0.050454 -0.054299 -0.047031 -0.052571 -0.033425 -0.035243 -0.027276 -0.015008 -0.014767 -0.029807 -0.019357 -0.027223 -0.036015 -0.025203 -0.027939 -0.007877 -0.001109 -0.005227 -0.003999 -0.010697 -0.016868 -0.016456 -0.018487 -0.008629  0.015259  0.009037  0.017640  0.012080  0.007992 -0.000293  0.015855  0.012048  0.009538  0.007727  0.020213  0.026757  0.026529  0.036255  0.043465  0.037656  0.021977  0.015545  0.001803  0.018512  0.032102  0.025598  0.026782  0.046457  0.045135  0.057550  0.040966  0.042690  0.056369  0.037146  0.041040  0.043903  0.053225  0.059823  0.068199  0.076368  0.071201  0.084847  0.072264  0.068131  0.074217  0.067147  0.081124  0.073280  0.103463  0.097042  0.104832  0.109236  0.108655  0.102062  0.086610  0.070059  0.053407  0.062062  0.070948  0.064974  0.055521  0.055137  0.048148  0.047184  0.034231  0.055017  0.107932  0.060634  0.053989  0.063130  0.060940  0.092446  0.027563  0.024510  0.026941  0.051341  0.063157  0.040213  0.046286  0.020911 -0.006044 -0.009020 -0.008355  0.028517  0.022838  0.003813  0.006019 -0.019919 -0.042731 -0.038798 -0.008082 -0.008098 -0.060944 -0.026939 -0.026682 -0.046161 -0.049482 -0.060528 -0.060718
         0.104703  0.083089 -0.110272 -0.127094 -0.129680 -0.126663 -0.122202  0.021660  0.040212  0.058051  0.080929  0.072373  0.088297  0.081788  0.093984 -0.122476 -0.123547 -0.122079 -0.124024 -0.117334 -0.116679 -0.114515 -0.103194 -0.096033 -0.003571  0.017682  0.025600  0.038676  0.055926  0.071744  0.078746  0.085811  0.088216 -0.123549 -0.120817 -0.115558 -0.105991 -0.102868 -0.098254 -0.086785 -0.078590 -0.072751 -0.065118 -0.055847 -0.020792 -0.005249 -0.003157 -0.003243  0.019462  0.020077  0.029520  0.032458  0.037379  0.052832  0.071472  0.076705 -0.113288 -0.104191 -0.095308 -0.093938 -0.080939 -0.073595 -0.068706 -0.060659 -0.052239 -0.041784 -0.045767 -0.035253 -0.051794 -0.035733 -0.044591 -0.072998 -0.066548 -0.090117 -0.082162  0.011796  0.003371 -0.015077 -0.020843 -0.029691 -0.053891 -0.034992 -0.059104 -0.032119 -0.019649 -0.017538 -0.029533 -0.011352 -0.006708  0.004337  0.031544  0.053341  0.066758  0.037303 -0.012525 -0.029306 -0.102710 -0.089959 -0.074457 -0.065776 -0.044252 -0.055968 -0.062779 -0.070704 -0.092853 -0.096136  0.055071 -0.012327 -0.052778 -0.075258 -0.081398 -0.092175 -0.077479 -0.109532  0.084314  0.054904 -0.108288 -0.119805 -0.128462 -0.122359 -0.123082  0.099664  0.064728  0.023182 -0.127205 -0.122083  0.059462  0.096892
         0.318235  0.364149  0.312792  0.336000  0.360611  0.383421  0.399917  0.416436  0.401174  0.387341  0.365098  0.375083  0.350893  0.363006  0.328174  0.313365  0.321735  0.339158  0.357714  0.365252  0.380437  0.397373  0.400651  0.414172  0.415730  0.402383  0.403662  0.395758  0.382834  0.366107  0.355648  0.340780  0.322598  0.315919  0.329173  0.336160  0.353592  0.359792  0.373010  0.369030  0.383631  0.395815  0.409998  0.405540  0.396323  0.380248  0.364024  0.356122  0.359100  0.376751  0.382082  0.395626  0.374610  0.348871  0.330347  0.317952  0.313031  0.324962  0.323285  0.343684  0.356371  0.352575  0.372623  0.378444  0.383935  0.385972  0.378995  0.379965  0.359245  0.377639  0.354343  0.335491  0.348504  0.314707  0.330059  0.313122  0.323464  0.313879  0.335967  0.330382  0.310572  0.320553  0.317856  0.356738  0.358170  0.367508  0.378693  0.338023  0.336620  0.340848  0.336706  0.331546  0.316908  0.357353  0.355455  0.323132  0.312808  0.333361  0.344156  0.357169  0.344246  0.395951  0.390971  0.377357  0.331856  0.317976  0.338603  0.364080  0.409734  0.414794  0.409295  0.390864  0.369479  0.342671  0.325355  0.372095  0.384006  0.398053  0.344500  0.329704  0.324182  0.326574  0.378121  0.414905  0.377912  0.324353  0.386863  0.335798
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N5  = [  0.276888 -0.056528 -0.388706 -0.350146 -0.179230 -0.063606  0.215526 -0.089536 -0.053158  0.001092  0.030683  0.036252  0.097170  0.155774  0.222595 -0.201482 -0.000462  0.216386  0.364458  0.437960  0.554521  0.510854  0.612768  0.549956  0.614393  0.667131  0.540115  0.442071  0.297312  0.278578  0.353543  0.182330  0.228730  0.093612  0.149169  0.370486  0.446862  0.496085  0.587441  0.506211  0.611137  0.614458  0.643201  0.642108  0.605609  0.722347  0.761938  0.732604  0.781339  0.770715  0.748655  0.630879  0.708156  0.634650  0.441484  0.423960  0.554860  0.577764  0.593228  0.573925  0.504928  0.598376  0.466966  0.506762  0.524157  0.568640  0.529083  0.534717  0.738262  0.752136  0.814650  0.625030  0.633336  0.612602  0.604738  0.759857  0.701383  0.732515  0.607064  1.005215  0.930137  1.020036  0.829779  0.858834  0.400419  0.437251  0.417137  0.472333  0.615020  0.779015  0.793708  0.678074  0.538625  0.724243  0.487937  1.027398  0.525242  0.600960  0.627114  0.583313  0.837251  0.625048  0.599634  0.499831  0.621941  0.556240  0.650403  0.522047  0.660900  0.612172  0.610205  0.626696  0.485855  0.464707  0.334838  0.523978  0.580122  0.470471  0.125369  0.101258  0.054031  0.114429  0.137934  0.140142  0.098547 -0.281638 -0.055586  0.075397
         0.950078  0.906183 -0.909084 -1.001468 -1.119097 -1.108008 -0.909898  0.626620  0.673020  0.666674  0.914237  0.773420  0.983562  0.920898  0.985074 -1.106360 -1.122932 -1.080153 -1.028170 -0.960502 -0.851796 -0.833530 -0.723855 -0.784291  0.356564  0.198235  0.308860  0.601289  0.749406  0.843176  0.874745  1.031481  1.023692 -1.091498 -1.068765 -0.928578 -0.752636 -0.739064 -0.720257 -0.755006 -0.700273 -0.753430 -0.696356 -0.706137  0.589253  0.450102  0.347076  0.364204  0.117882  0.140650  0.266357  0.377495  0.409797  0.587693  0.880223  0.911760 -0.733745 -0.790395 -0.686024 -0.772417 -0.759080 -0.722065 -0.737578 -0.692726 -0.663982 -0.381231 -0.594247  0.301299 -0.583677  0.190704 -0.461828 -0.718622 -0.719056 -0.662531 -0.741932  0.243179  0.284287  0.626054  0.726916  0.059955 -0.357359 -0.174705 -0.497475  0.062088  0.843293  0.789494  0.593364  0.779661  0.608471  0.161582  0.254335  0.569987  0.794514  0.358166  0.781775  0.071492 -0.795653 -0.783492 -0.722293 -0.736895 -0.481703 -0.680945 -0.705663 -0.756624 -0.752126 -0.689604  0.599146  0.766585 -0.686064 -0.740865 -0.746200 -0.725761 -0.682272 -0.851910  0.973536  0.659821 -0.801263 -0.803933 -1.110832 -1.106394 -1.114714  1.033199  0.779525  0.593848 -1.109100 -0.910383  0.655970  1.016592
         0.728100  0.931949 -0.186975 -0.051972  0.432150  0.575269  0.956401  1.211342  1.175432  1.179124  0.911336  1.083318  0.770510  0.873005  0.701641  0.273067  0.353632  0.508455  0.522258  0.647967  0.728544  0.826268  0.856472  0.852736  1.110773  1.124087  1.192564  1.091811  1.034151  0.929670  0.834696  0.596478  0.584407  0.554531  0.615105  0.799299  0.980341  0.957304  0.890388  0.928523  0.888212  0.809032  0.854646  0.841852  0.937411  0.935669  0.956241  0.980678  1.020348  1.028559  1.019623  1.092198  0.995656  0.941204  0.746234  0.698654  0.910158  0.820372  0.926696  0.849273  0.927739  0.887098  0.979141  0.997794  1.012526  1.172817  1.071068  1.197946  0.887447  1.074097  0.895915  0.862387  0.853258  0.933159  0.855131  1.010104  1.049116  0.753475  0.761095  0.756145  0.794337  0.707677  0.845358  0.985064  0.740522  0.816243  1.075773  0.799747  0.901425  1.013031  0.973006  0.911435  0.795298  1.006145  0.781681  0.703160  0.864141  0.802227  0.856241  0.883216  0.848135  0.898360  0.896868  0.930429  0.821524  0.954109  0.914494  0.772672  0.844893  0.832855  0.827146  0.832769  1.021450  0.846286  0.641117  0.976905  0.783677  0.916312  0.255064  0.470137  0.437344  0.639236  1.057740  1.218963  0.500145 -0.245460  1.189455  0.704070
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

%%% Ordered transformation matrices
% Global rotation operator
w0a = [0.999790 -0.020328 -0.002402]';
q0a = [0.0, 0.0, 0.0]';
t0b = twist_matrix_fast(w0a,q0a,sp_coords(4));

% Global translation operator
t0a = twist_matrix_fast(sp_coords(1:3),0,0);

w1a = [-0.009912 -0.999163 -0.039697]';
q1a = [0.068930 -0.011052 0.120007]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.244879 0.040916 -0.968690]';
q1b = [0.068930 -0.011052 0.120007]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.969503 0.000119 -0.245080]';
q1c = [0.068930 -0.011052 0.120007]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [-0.028984 -0.998972 -0.034868]';
q2a = [0.013719 -0.001827 -0.098396]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.007188 0.035090 -0.999358]';
q2b = [0.013719 -0.001827 -0.098396]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [0.999554 -0.028715 -0.008198]';
q2c = [0.013719 -0.001827 -0.098396]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.207089 -0.977666 -0.035818]';
q3a = [0.012823 0.002548 -0.222986]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [0.375414 0.113222 -0.919916]';
q3b = [0.012823 0.002548 -0.222986]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.903426 0.177058 0.390477]';
q3c = [0.012823 0.002548 -0.222986]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [0.101648 -0.991553 -0.080557]';
q4a = [0.041231 0.011116 -0.292597]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [-0.907033 -0.059114 -0.416888]';
q4b = [0.041231 0.011116 -0.292597]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [0.408605 0.115444 -0.905381]';
q4c = [0.041231 0.011116 -0.292597]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.009912 0.999163 0.039697]';
q5a = [0.068930 -0.011052 0.120007]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.001871 -0.039717 0.999209]';
q5b = [0.068930 -0.011052 0.120007]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.999949 -0.009830 -0.002264]';
q5c = [0.068930 -0.011052 0.120007]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.090612 0.995094 0.039724]';
q6a = [0.069218 -0.017163 0.273749]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [-0.682818 -0.091114 0.724884]';
q6b = [0.069218 -0.017163 0.273749]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.724947 0.038559 0.687724]';
q6c = [0.069218 -0.017163 0.273749]';
t6c = twist_matrix_fast(w6c,q6c,Theta(18));

%%% Construct kinematic chains of transformations
% Generate new_config and new_norms
U1 = t0a*t0b*t1a*t1b*t1c*I1;
V1 = t0a*t0b*t1a*t1b*t1c*N1;
V1 = V1-U1;

U2 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*I2;
V2 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*N2;
V2 = V2-U2;

U3 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*t4a*t4b*t4c*I3;
V3 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*t4a*t4b*t4c*N3;
V3 = V3-U3;

U4 = t0a*t0b*t5a*t5b*t5c*I4;
V4 = t0a*t0b*t5a*t5b*t5c*N4;
V4 = V4-U4;

U5 = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*I5;
V5 = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*N5;
V5 = V5-U5;

%%% Construct complete update
new_config = [U1(1:3,:) U2(1:3,:) U3(1:3,:) U4(1:3,:) U5(1:3,:)];
new_norms  = [V1(1:3,:) V2(1:3,:) V3(1:3,:) V4(1:3,:) V5(1:3,:)];

% Update center of mass estimate
new_ctrm = mean(new_config,2);

%%% Twist matrix function
function [twist] = twist_matrix_fast(w,q,theta)
% Overview:
% This code generates an exponential map matrix for rotation and translation
% using the Rodrigues' formula for the matrix exponential, from "A Mathematical 
% Introduction to Robotic Manipulation" by Murray, Li, and Sastry (1994) p. 28. 
% The equation for the exponential map for twists is from Ibid. 42.
%
% This code has very little error checking to increase speed.
%
% [twist] = twist_matrix_fast(w,q,theta)
% INPUTS %
% w: a 3d vector pointing: 1) in the direction of translation or 2) 
%      along the axis of rotation relative to the origin.
% q: a 3d point along the axis of rotation, providing the offset
% theta: the amount of rotation, in radians
%
% OUTPUTS %
% twist: the resulting matrix for left multiplication
%
% I treat 2 cases: 1) pure translation and 2) pure rotation
if numel(q) < 3 && (q == 0 || theta == 0)
    
    % we want column vectors
    if size(w,1)<size(w,2)
        w = w';
    end
    
    % 1) pure translation
	% pure translation in absolute frame
	twist = [eye(3) w;zeros(1,3) 1];
else
    % 2) pure rotation twist
    % correct for case where w is the origin
    if sum(w) == 0
        w = q;
        W = 0;
    else
        W = 1;
    end
    
    % Generate cross product operator so(3)
    w_hat = [0   -w(3) w(2);
             w(3) 0   -w(1);
            -w(2) w(1) 0];
    
    v = -w_hat*q;
    % Generate rotation matrix using the Rodrigues' equation pg. 28
    Id3x3  = eye(3);
    w_hat2 = w*w'-W*Id3x3;
    R      = Id3x3+w_hat*sin(theta)+w_hat2*(1-cos(theta));
    
    % Generate relative frame twist, pg. 42
    twist  = [R (Id3x3-R)*w_hat*v+w*w'*v*theta;
              zeros(1,3) 1];
end
