function [new_config, new_ctrm, new_norms]  = buddy140_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 18 x 1 angles about joint 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.044081  0.047957  0.046628  0.044177  0.046973  0.041779  0.040622  0.041175  0.035240  0.034748  0.037021  0.035800  0.036003  0.036224  0.036575  0.036652  0.036141  0.034849  0.024070  0.019157  0.008776  0.019864  0.008735  0.014492  0.020627  0.025182  0.029185  0.031698  0.035491  0.037868  0.041423  0.044743  0.047164  0.046583  0.050937  0.035276  0.028400  0.023336  0.042767  0.050300  0.044358  0.033453  0.035425  0.037871  0.031229  0.026803  0.020465  0.021162  0.026447  0.026127  0.031478  0.030293  0.030601  0.027489  0.028188  0.022225  0.022667  0.028554  0.022036  0.019128  0.017643  0.010965  0.008436 -0.010017 -0.005280 -0.010124 -0.017634 -0.005005 -0.014465 -0.006459 -0.006689 -0.012043  0.006231 -0.005899 -0.009183 -0.000917 -0.007002 -0.003068 -0.004746 -0.007794 -0.014313 -0.033063 -0.020626 -0.007614 -0.015627 -0.027616 -0.026054 -0.016569 -0.020587 -0.010154 -0.029006 -0.006167 -0.008660  0.011834  0.007152  0.010409 -0.010674  0.007439  0.016686  0.008565  0.007182  0.017701  0.004813  0.004389  0.019722 -0.000514 -0.000514  0.015410  0.018507  0.029138  0.030474  0.029094  0.020097  0.029223  0.033407  0.041578  0.033195  0.020169  0.003778  0.002473  0.019578  0.032496  0.033358  0.038842  0.039217  0.043870  0.047798  0.040843
        -0.044591 -0.053452 -0.054579 -0.056610 -0.061246 -0.051879 -0.046948 -0.057483 -0.052142 -0.038952 -0.035517 -0.030050 -0.029433 -0.028723 -0.028006 -0.027434 -0.028133 -0.030035 -0.022839 -0.146143 -0.131814 -0.128004 -0.112214 -0.101020 -0.104277 -0.100010 -0.100871 -0.105586 -0.106340 -0.091914 -0.100734 -0.084305 -0.082402 -0.076051 -0.079399 -0.126988 -0.132551 -0.133342 -0.111215 -0.060146 -0.073832 -0.090613 -0.084098 -0.078961 -0.073083 -0.084128 -0.079300 -0.065893 -0.060608 -0.052260 -0.059419 -0.038435 -0.024628  0.002083 -0.005913  0.009559 -0.002265 -0.010884 -0.008468 -0.011128 -0.007704 -0.008945 -0.011773 -0.141268 -0.133064 -0.128064 -0.119134 -0.111186 -0.106745 -0.104938 -0.086116 -0.080098  0.034544  0.031093  0.028047  0.015283  0.016604  0.008018  0.005097  0.002223  0.008441  0.050928  0.048055  0.038300  0.035074  0.038305  0.029129  0.018269  0.013852  0.023598  0.050214  0.008392  0.020207  0.001839  0.009656  0.012199  0.031330 -0.072535 -0.073269 -0.078413 -0.091371 -0.084140 -0.101091 -0.108233  0.024584 -0.093501 -0.155669  0.003049  0.006894 -0.010805 -0.016086 -0.011355 -0.050011 -0.065590 -0.062491 -0.078975 -0.116491 -0.120181 -0.117739 -0.141183 -0.019020 -0.021174 -0.042016 -0.039545 -0.039230 -0.065303 -0.058228 -0.038123
         0.098196  0.088529  0.076702  0.064263  0.069163  0.053836  0.033806  0.025398  0.010647  0.011326  0.023145  0.031630  0.044723  0.057794  0.070799  0.083925  0.097902  0.112618  0.126394  0.027385  0.009339  0.014616 -0.003687 -0.006803  0.003312  0.008084  0.019399  0.028817  0.037191  0.033729  0.046983  0.050505  0.060859  0.061382  0.069667  0.039209  0.031235  0.021235  0.051391  0.082492  0.045382  0.019611  0.016984  0.020137  0.004076 -0.001001 -0.014522 -0.015529 -0.006047 -0.007905  0.003483 -0.001109  0.002801  0.031719  0.046427  0.052936  0.071387  0.070989  0.092477  0.108496  0.103373  0.126183  0.141324  0.009837  0.003598 -0.002115 -0.017763 -0.016293 -0.025848 -0.022567 -0.034258 -0.039998  0.048803  0.066734  0.082614  0.093918  0.107945  0.120181  0.132881  0.148723  0.146738  0.068803  0.062174  0.058486  0.080697  0.096010  0.119336  0.128678  0.140871  0.097815  0.069181  0.128152  0.102828  0.093944  0.085990  0.070326  0.077881 -0.035153 -0.022163 -0.031444 -0.023182 -0.015827 -0.016507 -0.010625  0.043830 -0.026875  0.021991  0.080115  0.062831  0.052851  0.036722  0.026241 -0.019841 -0.000590  0.007094  0.031576  0.034304  0.011985 -0.003746  0.012405  0.128941  0.049074  0.007299  0.028769  0.046318  0.052605  0.074088  0.107786
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.913454  0.913896  0.952672  0.992845  0.948412  1.026179  1.022757  1.014371  0.930147  0.964451  0.990119  0.963034  0.964533  0.967985  0.973606  0.959457  0.903525  0.812940  0.730408  0.679579  0.469647  0.660367  0.588720  0.682357  0.749162  0.897457  0.922334  0.920589  0.904846  0.908088  0.954342  1.019728  0.991626  0.995188  0.932287  0.858835  0.862462  0.866393  0.931421  0.939425  1.039938  0.868544  0.918487  0.970134  0.905175  0.916475  0.824232  0.893279  0.907677  0.916979  0.918667  0.985262  1.013185  1.023463  1.007946  0.838137  0.806069  0.889786  0.741838  0.723968  0.721967  0.714691  0.712132 -0.037850  0.192357  0.170151  0.151024  0.511803  0.308730  0.446248  0.327647  0.378462  0.591289  0.662593  0.681983  0.702811  0.694439  0.700636  0.682175  0.638835  0.487743 -0.020339  0.539665  0.611705  0.626342  0.205399  0.408507  0.582603  0.308328  0.688373  0.267818  0.685870  0.693181  0.715640  0.710877  0.707542  0.674297  0.580228  0.870182  0.605454  0.547536  0.773832  0.550121  0.565556  0.774198  0.453218  0.275997  0.730164  0.763504  0.988017  0.991051  1.009090  0.936962  0.912551  0.914322  0.984950  0.898600  0.723680  0.529259  0.335127  0.723615  0.962667  0.956591  1.005134  1.007676  1.032345  0.955041  0.889494
         0.445796  0.424511  0.343123  0.141804  0.182123  0.116383  0.135698  0.018921 -0.037295  0.080703  0.245810  0.343059  0.341013  0.331568  0.318466  0.357644  0.459945  0.575006  0.642786 -0.747249 -0.612166 -0.422516 -0.602033 -0.578580 -0.519930 -0.418013 -0.378347 -0.302469 -0.234600 -0.413261 -0.222147 -0.144664 -0.002045  0.046005  0.043143 -0.261805 -0.411832 -0.422939 -0.135390  0.357554 -0.057047 -0.491839 -0.390192 -0.283724 -0.187761 -0.300794 -0.274676 -0.101983 -0.107829 -0.168583 -0.074430 -0.076603  0.090912  0.087910  0.191322  0.426314  0.588884  0.491537  0.648627  0.655921  0.659714  0.658944  0.656113 -0.749524 -0.761561 -0.858046 -0.829364 -0.680905 -0.613269 -0.642896 -0.539539 -0.578045  0.589507  0.686437  0.706841  0.683150  0.686147  0.675904  0.684495  0.710052  0.827515  0.962668  0.739481  0.624073  0.756901  0.989785  0.817132  0.738072  0.832449  0.696167  0.934655  0.683319  0.689545  0.669670  0.677533  0.663187  0.712623 -0.400705 -0.219130 -0.466149 -0.621205 -0.413485 -0.658438 -0.646378  0.558774 -0.601718 -0.778220  0.661679  0.574751  0.271920  0.260682  0.187658 -0.215207 -0.101830 -0.071904 -0.282429 -0.191256 -0.397861 -0.628274 -0.690176  0.648627  0.343956  0.000241  0.203866  0.209776  0.076827  0.305258  0.489972
         0.037260 -0.058780 -0.067925 -0.182030 -0.288858  0.002410 -0.011473 -0.191515 -0.435357 -0.337005 -0.088479 -0.000407  0.020197  0.012937  0.026878  0.095971  0.194981  0.281442  0.367289 -0.422631 -0.736892 -0.694621 -0.654606 -0.577673 -0.541179 -0.363413 -0.334567 -0.384837 -0.440063 -0.339706 -0.342680 -0.163412 -0.257786 -0.230598 -0.386628 -0.511767 -0.444520 -0.431962 -0.406549 -0.104547 -0.047021 -0.356741 -0.338693 -0.278120 -0.468224 -0.402921 -0.576471 -0.503494 -0.476371 -0.447054 -0.457678 -0.295350 -0.142732  0.057597  0.080667  0.453690  0.263286  0.147487  0.316330  0.349838  0.345202  0.368454  0.383688 -0.783416 -0.748686 -0.661377 -0.701231 -0.655300 -0.825209 -0.733663 -0.860471 -0.814307  0.640177  0.418360  0.330664  0.336242  0.352264  0.362523  0.390869  0.433060  0.424330  0.479374  0.518248  0.581281  0.339235  0.296978  0.555463  0.479215  0.611735  0.342171  0.429264  0.384194  0.346565  0.336138  0.328295  0.370692  0.336059 -0.786297 -0.522433 -0.733856 -0.676859 -0.581334 -0.642609 -0.639505  0.425154 -0.758888 -0.710110  0.315343  0.412858  0.077734  0.010417  0.026680 -0.383255 -0.467943 -0.466087 -0.230447 -0.461159 -0.642207 -0.684352 -0.754373  0.370972  0.010824 -0.374611 -0.055082  0.037186  0.000487 -0.137544  0.137921
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.020749  0.021985  0.023727  0.027888 -0.008568 -0.007002 -0.011395 -0.019410 -0.006903 -0.005844 -0.001418 -0.003698 -0.001953 -0.004534 -0.035152 -0.034654 -0.040141 -0.037708 -0.045359 -0.030578 -0.035051 -0.027127 -0.030500 -0.048994 -0.021335 -0.021755 -0.015960 -0.017924 -0.024473 -0.030945  0.011242  0.011611  0.017344  0.007017  0.007175  0.007498  0.003249  0.000315  0.014511  0.014334  0.016129  0.019299  0.024260  0.021050  0.028192  0.027083  0.025690  0.030816  0.028874  0.030492  0.029796  0.029740  0.027857  0.027354  0.021889 -0.001683  0.009915  0.009156  0.025039 -0.013870 -0.021173 -0.005448  0.028764  0.019111
        -0.024972 -0.005988  0.008332  0.002163 -0.049962 -0.042475 -0.039756 -0.031977 -0.018654 -0.006171  0.086760  0.075774  0.063424  0.059128  0.073700  0.077515  0.095291  0.106584 -0.011383 -0.031031 -0.051531 -0.065677 -0.052101  0.118210  0.092384  0.083119  0.072834  0.070260  0.077027  0.094650  0.045546  0.072831  0.068591  0.002509 -0.005321 -0.012184 -0.034018 -0.038730 -0.033107 -0.016815 -0.003393  0.007137  0.014331  0.015069  0.056094  0.055654  0.054931  0.034276  0.042314  0.020931  0.021004  0.023802  0.023425  0.028801  0.019137 -0.025003 -0.011593  0.006086  0.041571  0.096836 -0.023743 -0.050041  0.015673 -0.022360
        -0.041786 -0.041780 -0.044697 -0.022470 -0.061952 -0.069065 -0.080580 -0.093617 -0.093091 -0.101404 -0.020003 -0.000861  0.011484  0.024812  0.048488  0.042288  0.018262  0.004670 -0.104999 -0.097353 -0.087916 -0.066156 -0.085734 -0.003215  0.003142  0.014469  0.020898  0.032513  0.030526  0.012902  0.017909 -0.019830 -0.025047 -0.088604 -0.074068 -0.069136 -0.062294 -0.062565 -0.047429 -0.055624 -0.056697 -0.057494 -0.051499 -0.067964 -0.049798 -0.035495 -0.021316 -0.022068 -0.009798 -0.008325 -0.035088 -0.042966 -0.055152 -0.066715 -0.073210 -0.079262 -0.065249 -0.087864  0.000375 -0.013544 -0.099278 -0.058902 -0.036362 -0.045418
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.870070  0.927603  0.860200  1.023458  0.566630  0.665570  0.650401  0.354028  0.655580  0.651340  0.694679  0.635830  0.652956  0.692917  0.035519 -0.001529  0.124756  0.260901 -0.530302  0.056183  0.206403  0.600583  0.343858 -0.332444  0.651302  0.651947  0.674808  0.657758  0.567629  0.526917  0.698050  0.712031  0.759476  0.848849  0.842494  0.833972  0.751336  0.726687  0.823208  0.881339  0.913655  0.914298  0.861700  0.872242  0.999745  0.971613  0.917368  1.028105  0.945405  1.025258  1.019198  1.013009  0.948918  0.954885  0.862571  0.843008  0.845841  0.854005  0.812530  0.681067  0.515481  0.566108  1.003591  0.853658
        -0.229566 -0.056602 -0.366002 -0.067888 -0.649927 -0.627111 -0.631834 -0.503733 -0.434883 -0.534757  0.735220  0.660948  0.673369  0.725943  0.703338  1.013614  0.838419  0.847138 -0.554670 -0.530942 -0.627289 -0.687945 -0.659941  0.682908  0.642925  0.675518  0.723361  0.748180  0.814658  0.711326  0.687285  0.716067  0.695218 -0.474469 -0.381989 -0.286242 -0.444553 -0.497305 -0.382172 -0.176116 -0.242384 -0.379336 -0.459160 -0.455211  0.278300  0.369275  0.480502  0.105555  0.372344 -0.073894 -0.114087 -0.140787 -0.314587 -0.223440 -0.430625 -0.385559 -0.265857 -0.473374  0.502340  0.714992 -0.532477 -0.613509 -0.181289 -0.211123
        -0.528402 -0.462843 -0.444930 -0.085181 -0.618000 -0.522769 -0.540438 -0.892360 -0.715882 -0.638719  0.288134  0.497712  0.457647  0.287339  0.822155  0.392461  0.666775  0.606677 -0.790323 -0.959073 -0.869070 -0.533877 -0.786011  0.771880  0.497573  0.456270  0.336579  0.322133  0.355049  0.568330  0.359178  0.289460  0.212816 -0.341211 -0.474522 -0.560899 -0.583665 -0.574516 -0.520886 -0.527774 -0.427277 -0.280241 -0.324447 -0.301009  0.032113  0.061994  0.132957 -0.040324  0.216146  0.029732 -0.088322 -0.121022 -0.248528 -0.342506 -0.374819 -0.474857 -0.551642 -0.325241  0.409705  0.353805 -0.772474 -0.655415 -0.140852 -0.563008
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.022143 -0.006723 -0.007853 -0.002709  0.002509  0.005379 -0.068024 -0.074204 -0.067133 -0.067044 -0.068624 -0.074706 -0.073941 -0.059142 -0.066453 -0.066478 -0.066012 -0.060570 -0.060661 -0.073960 -0.067193 -0.074205 -0.088744 -0.089362 -0.084266 -0.089039 -0.090421 -0.097273 -0.088798 -0.090751 -0.088135 -0.095761 -0.094167 -0.094307 -0.100620 -0.096178 -0.084239 -0.103417 -0.112864 -0.123768 -0.121228 -0.124251 -0.121340 -0.117338 -0.121347 -0.112785 -0.119941 -0.121502 -0.125872 -0.126221 -0.113951 -0.110169 -0.111269 -0.108698 -0.104523 -0.114920 -0.122846 -0.120925 -0.115949 -0.128418 -0.129605 -0.129163 -0.127817 -0.124408 -0.129882 -0.128396 -0.130549 -0.110260 -0.125885 -0.100600 -0.107789 -0.110251 -0.101424 -0.086012 -0.099312 -0.079757 -0.041564 -0.034583 -0.041004 -0.043676 -0.036839 -0.039254 -0.041910 -0.039578 -0.033645 -0.042937 -0.033977 -0.042566 -0.024181 -0.033837 -0.031801 -0.033588 -0.034166 -0.048425 -0.048365 -0.041845 -0.004058  0.008400  0.010289  0.001912 -0.006030  0.000802 -0.000695  0.001829 -0.006146 -0.007517 -0.007464  0.001206 -0.008062 -0.003475 -0.003643 -0.002691  0.004267  0.000393  0.011001 -0.000003 -0.004808  0.008913  0.009580  0.007848  0.003827  0.023277  0.023974  0.011813  0.008307  0.022537  0.022291  0.032892  0.033481  0.032640  0.028474  0.035070  0.022236  0.031079  0.035872  0.043827  0.037191  0.050606  0.050746  0.048869  0.040711  0.040737  0.043770  0.035315  0.041139  0.033200  0.048854  0.045764  0.035671  0.040015  0.063354  0.059815  0.048839  0.056001  0.046160  0.046433  0.059548  0.064189  0.064492  0.064701  0.078212  0.069546  0.062502  0.077202  0.063115  0.058936  0.058569  0.066094  0.086131  0.082462  0.087741  0.089160  0.090358  0.083178  0.078860  0.074550  0.057088  0.080096  0.052489  0.068611  0.067791  0.067656  0.053335  0.052480  0.022892  0.023258  0.054800  0.023322  0.022591  0.029172  0.027504  0.032178  0.033003  0.030265  0.027955  0.010664  0.018048  0.019201  0.022122  0.041213  0.015816  0.027545  0.027931 -0.018615 -0.008241 -0.019301 -0.021608  0.014056  0.015094 -0.013416 -0.017789 -0.019871 -0.023368 -0.021836 -0.018613 -0.022706 -0.028842 -0.026845 -0.020870 -0.013704 -0.016670 -0.009739 -0.009103 -0.009512 -0.025645 -0.013306 -0.011789 -0.026053 -0.027888  0.013984  0.005731 -0.008560  0.012385 -0.040942 -0.045824 -0.052043 -0.043998 -0.048657 -0.050267 -0.056059 -0.046258 -0.028891 -0.051813 -0.042883 -0.051869 -0.061587 -0.079679 -0.054606 -0.053887 -0.082467 -0.082179 -0.053504 -0.055531 -0.001505  0.000166 -0.028556
         0.053667  0.056215  0.067811  0.078287  0.086857  0.087862  0.062914  0.074004  0.089352  0.103074  0.110585  0.122570  0.136525  0.142425  0.151506  0.155895  0.158387  0.161755  0.164835  0.165485  0.167142  0.163684  0.158778  0.166761  0.167339  0.168375  0.166524  0.166544  0.163751  0.159783  0.149490  0.137478  0.116350  0.102567  0.094050  0.084681  0.070629  0.093126  0.109430  0.115145  0.111101  0.123124  0.124592  0.141813  0.152020  0.157198  0.157918  0.163239  0.160886  0.152955  0.166908  0.170127  0.171115  0.168447  0.167384  0.162776  0.149779  0.130537  0.122625  0.128389  0.133897  0.136732  0.138869  0.136675  0.151997  0.131381  0.149749  0.152892  0.131494  0.087343  0.143346  0.163962  0.168087  0.169473  0.158033  0.159331  0.182047  0.176028  0.177057  0.173782  0.179452  0.177430  0.173381  0.172173  0.172634  0.160138  0.145290  0.149416  0.129409  0.116017  0.109505  0.096954  0.082197  0.081434  0.067919  0.056354  0.089168  0.091938  0.098101  0.110388  0.130186  0.142195  0.149548  0.171757  0.171704  0.179930  0.192743  0.210744  0.206906  0.217461  0.217932  0.216551  0.219994  0.212125  0.220328  0.206355  0.184046  0.183608  0.170684  0.164452  0.144881  0.115304  0.104637  0.094704  0.091969  0.097798  0.095124  0.098242  0.091598  0.090271  0.088391  0.089267  0.097836  0.107262  0.228284  0.214635  0.198500  0.197376  0.184785  0.178289  0.165876  0.152346  0.133446  0.124782  0.111404  0.105878  0.110482  0.104711  0.099769  0.101721  0.102556  0.096626  0.093318  0.089866  0.087551  0.087359  0.205269  0.184147  0.171409  0.158102  0.159485  0.154667  0.133198  0.132525  0.124067  0.105968  0.111113  0.105211  0.112645  0.104271  0.168970  0.121448  0.134481  0.161229  0.101743  0.103160  0.129100  0.151722  0.208732  0.093503  0.094924  0.096618  0.104954  0.115503  0.128617  0.144055  0.152834  0.157393  0.170173  0.186204  0.193806  0.207224  0.215586  0.216984  0.231583  0.223235  0.231396  0.126969  0.118440  0.086448  0.091708  0.099141  0.104630  0.164245  0.157781  0.167856  0.169012  0.153086  0.185328  0.193194  0.196126  0.196221  0.192942  0.191809  0.189511  0.179437  0.177009  0.158145  0.152285  0.148058  0.131727  0.118128  0.112928  0.208485  0.186513  0.204821  0.204365  0.190155  0.187071  0.218114  0.226927  0.159445  0.091644  0.098198  0.116475  0.132984  0.142236  0.154731  0.161134  0.158474  0.169670  0.178487  0.174139  0.183649  0.175177  0.171429  0.166433  0.162990  0.155407  0.150872  0.124018  0.089620  0.083121  0.060837  0.072419  0.056069
        -0.173586 -0.169909 -0.173515 -0.167762 -0.169537 -0.158518 -0.170568 -0.172205 -0.174258 -0.174751 -0.175405 -0.176046 -0.176895 -0.175862 -0.169682 -0.159512 -0.153756 -0.148279 -0.136135 -0.115848 -0.110697 -0.096080 -0.103867 -0.111542 -0.132265 -0.141158 -0.152581 -0.166720 -0.161054 -0.171632 -0.175712 -0.175152 -0.174061 -0.173254 -0.169782 -0.167847 -0.166143 -0.167553 -0.166943 -0.153488 -0.144276 -0.142268 -0.130756 -0.117082 -0.129720 -0.123353 -0.135901 -0.146852 -0.168299 -0.170955 -0.171294 -0.169998 -0.153930 -0.139599 -0.128582 -0.135734 -0.172384 -0.172238 -0.171726 -0.160703 -0.160042 -0.148333 -0.134505 -0.125916 -0.167267 -0.155981 -0.148924 -0.115449 -0.131261 -0.166451 -0.174265 -0.173210 -0.168144 -0.114766 -0.105087 -0.095900 -0.085513 -0.100593 -0.114943 -0.129823 -0.121870 -0.134934 -0.148512 -0.162855 -0.177640 -0.170650 -0.195974 -0.182188 -0.203769 -0.187067 -0.185077 -0.179681 -0.176458 -0.174629 -0.174445 -0.174564 -0.178692 -0.188433 -0.210879 -0.215586 -0.224078 -0.235723 -0.235906 -0.230852 -0.218048 -0.210863 -0.203775 -0.209302 -0.190728 -0.185035 -0.171708 -0.157327 -0.149031 -0.135772 -0.139114 -0.130279 -0.124057 -0.123239 -0.124581 -0.124565 -0.114928 -0.119823 -0.134054 -0.149857 -0.167428 -0.164055 -0.176425 -0.169752 -0.193024 -0.198674 -0.206206 -0.223664 -0.222300 -0.237930 -0.149460 -0.131162 -0.125742 -0.125862 -0.125772 -0.125631 -0.123751 -0.123813 -0.126785 -0.125322 -0.131304 -0.139806 -0.136892 -0.151644 -0.164963 -0.159744 -0.162739 -0.182424 -0.189863 -0.204677 -0.226620 -0.212177 -0.135803 -0.126363 -0.126169 -0.126357 -0.128711 -0.127414 -0.132490 -0.140167 -0.136520 -0.152495 -0.142141 -0.157712 -0.170079 -0.184382 -0.130601 -0.161781 -0.148368 -0.130365 -0.184650 -0.168001 -0.131639 -0.131403 -0.131404 -0.210031 -0.196780 -0.188748 -0.153572 -0.133672 -0.120638 -0.124694 -0.124729 -0.125017 -0.123478 -0.124217 -0.125029 -0.127816 -0.133095 -0.135788 -0.155916 -0.143087 -0.162034 -0.246251 -0.239995 -0.217805 -0.204864 -0.162815 -0.140789 -0.102325 -0.114764 -0.108811 -0.086773 -0.124402 -0.122957 -0.130895 -0.143089 -0.155357 -0.162675 -0.176609 -0.189101 -0.190377 -0.182469 -0.198561 -0.210343 -0.221398 -0.215483 -0.211387 -0.206334 -0.183293 -0.177239 -0.156770 -0.148384 -0.149705 -0.136476 -0.137634 -0.183407 -0.222938 -0.194261 -0.178122 -0.181508 -0.181732 -0.184581 -0.171058 -0.160279 -0.157596 -0.142107 -0.115512 -0.107879 -0.088773 -0.095284 -0.101551 -0.130702 -0.148591 -0.164326 -0.174750 -0.175294 -0.174840 -0.174346 -0.167182 -0.164353 -0.174342
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.159082  0.405440  0.437374  0.663098  0.711859  0.907670 -0.402346 -0.317626 -0.084825 -0.159979 -0.193217 -0.202887 -0.118601 -0.201902  0.065688  0.014034 -0.118292 -0.255090 -0.210388 -0.064041 -0.471029 -0.676244 -0.105990 -0.326225  0.131855  0.266794  0.329222  0.317618  0.358855  0.255817 -0.029475 -0.203006 -0.129368 -0.280763 -0.572097 -0.587615 -0.580136 -0.775841 -0.669941 -1.052521 -1.026162 -1.040795 -0.939079 -0.975808 -0.856211 -0.729797 -0.800964 -0.744409 -0.744049 -0.629984 -0.481878 -0.004762 -0.273068 -0.571433 -0.626367 -0.723185 -0.396903 -0.408904 -0.484170 -1.087403 -1.104917 -1.095253 -1.106652 -0.934633 -0.998075 -1.094565 -1.104111 -0.765977 -1.021375 -0.736842 -0.203848 -0.236504  0.246391  0.005293 -0.649351 -0.708020  0.091823  0.228760 -0.342900 -0.666313 -0.455388 -0.728682 -0.788029 -0.813619 -0.852967 -0.798752 -0.809974 -0.737478 -0.780127 -0.473632 -0.423019 -0.284254 -0.172350 -0.118764 -0.065737 -0.085151  0.115540 -0.134769 -0.543028 -0.570749 -0.580792 -0.693604 -0.753740 -0.813397 -0.774608 -0.773692 -0.802736 -0.860177 -0.857108 -0.784635 -0.715709 -0.621741 -0.641666 -0.702183  0.120599 -0.577476 -0.019634 -0.057548  0.023994  0.169829  0.606437  0.595471  0.417021  0.665103  0.527117  0.244435  0.197218  0.150067  0.057482  0.041113 -0.038491 -0.332921 -0.503510 -0.550661 -0.044389  0.202582 -0.020229  0.172602  0.083806  0.115353  0.125718  0.016304  0.178905  0.319557  0.352385  0.238254  0.350948  0.158475  0.149957  0.152898  0.176820  0.205529  0.144024  0.277354 -0.149188  0.109186  0.699396  0.181687  0.112375  0.202317  0.375904  0.269600  0.450347  0.516944  0.544133  0.220933  0.441468  0.312426  0.790148  0.633907  0.969392  0.870638  0.720944  0.527286  0.537313  0.461563  0.458582  0.502546  0.615987  0.511022  0.350964  0.335164  0.152382  0.516194  0.541931  0.259238  0.211224 -0.038917 -0.041581  0.140301  0.125019 -0.240823 -0.197669 -0.291861 -0.121622 -0.021331 -0.428994 -0.615477 -0.592434 -0.008860 -0.411961  0.217246  0.260804  0.717086  0.614638  0.661643  0.667606  0.240183  0.019466 -0.523987 -0.663004 -0.736753 -0.740909 -0.806694 -0.808170 -0.790452 -0.822959 -0.795159 -0.785880 -0.773438 -0.692067 -0.620445 -0.631763 -0.863545 -0.824218 -0.795574 -0.738068 -0.677037 -0.582922  0.221957 -0.655978 -0.781169 -0.215657 -0.260653 -0.398438 -0.506899 -0.729481 -0.535654 -0.653985 -0.483820 -0.688248  0.084841 -0.373893 -0.078871 -0.418594 -0.496697  0.050216 -0.397667 -0.337877  0.098754 -0.194361 -0.146870 -0.091051  0.867872  0.681039  0.023905
        -0.059833 -0.439137  0.076637 -0.135893 -0.588076 -0.324644 -0.284370 -0.159721  0.079152  0.015335 -0.004388  0.014143  0.198106  0.594335  0.919968  1.103173  1.024917  1.095039  1.127478  1.131288  1.074159  0.961317  0.868259  0.939326  1.143653  1.091314  1.054489  1.012253  1.028369  0.806500  0.329266  0.051263  0.093332  0.000876 -0.125088 -0.414588 -0.448300 -0.239598 -0.312994 -0.254314 -0.198796 -0.178688 -0.386689  0.331056  0.649643  0.778027  0.778810  0.853463  0.784097  0.231823  0.579290  1.053297  1.147830  1.031062  0.944123  0.879864  0.186565 -0.102569 -0.174594 -0.151675 -0.062774 -0.095770  0.095665  0.078718  0.226432 -0.098940  0.276561  0.705847 -0.221322 -0.440997  0.103549  0.399746  1.027071  1.048047  0.868522  0.934913  1.072359  1.127156  1.129476  0.946540  1.084011  0.901465  0.828271  0.768506  0.558498  0.637608  0.347979  0.465791 -0.254490 -0.271484 -0.274013 -0.167257 -0.072458  0.018809  0.062512  0.012161 -0.704774 -0.884808 -0.584652 -0.446245 -0.283629 -0.102430  0.096324  0.493755  0.507977  0.582898  0.439959  0.550895  0.648215  0.829459  0.919334  0.973871  0.972154  0.812144  0.989463  0.670304  0.361129  0.138202  0.099111  0.238786  0.651929 -0.076156 -0.784012 -0.660321 -0.762911 -0.862875 -0.862696 -0.864968 -0.875557 -0.882075 -0.895282 -0.700964 -0.484864 -0.481681  1.064738  0.955307  0.272365  0.335915  0.182215  0.216463  0.222338  0.052020  0.019359  0.157922 -0.596498 -0.840875 -0.687503 -0.852849 -0.863357 -0.851193 -0.829348 -0.854795 -0.846658 -0.870326 -0.856699 -0.894357  0.839600  0.304089  0.157239  0.118451  0.001604  0.028802 -0.225955 -0.319426 -0.230077 -0.816615 -0.612755 -0.781300 -0.539113 -0.718644 -0.039899 -0.329739 -0.350609  0.024468 -0.776220 -0.757238 -0.154459 -0.144054  0.747151 -0.790668 -0.850740 -0.841054 -0.847056 -0.361023  0.302300  0.306503  0.118868  0.072157  0.091322  0.282538  0.374178  0.571175  0.875602  0.902648  1.026813  1.090954  1.077347 -0.411004 -0.458144 -0.911685 -0.798303 -0.861382 -0.850536  0.834137  0.624733  0.755362  0.878914  0.370417  0.192815  0.806985  0.925034  0.889759  0.888072  0.769201  0.618101  0.553322  0.591635  0.492931  0.434349  0.275805 -0.234096 -0.440142 -0.487877  0.686253  0.719784  0.816367  0.847183  0.946805  1.003119  0.932008  0.976658  0.491810 -0.880959 -0.110810 -0.155309  0.160720  0.581770  0.919224  0.764686  0.831957  0.924870  1.061270  1.114666  1.144464  1.103845  1.064021  1.155096  1.079753  1.044536  0.515857  0.020504  0.001565  0.058186 -0.123873  0.010044  0.022276
        -1.150456 -0.934597 -1.068889 -0.882484 -0.372731 -0.283858 -1.046709 -1.113544 -1.174049 -1.166550 -1.160929 -1.161852 -1.173997 -1.056428 -0.795784 -0.469646 -0.650135 -0.450179 -0.361746  0.143237 -0.230014 -0.132551  0.600646  0.477558 -0.142343 -0.288014 -0.340781 -0.502341 -0.389180 -0.851079 -1.157669 -1.165639 -1.173176 -1.150440 -1.023999 -0.881444 -0.862419 -0.828720 -0.881942 -0.183801  0.147372  0.120135  0.133619  0.359590  0.331092  0.360246  0.252301  0.221355 -0.647324 -1.031190 -1.004703 -0.627054 -0.013041  0.064781  0.224059  0.204560 -1.133394 -1.101070 -1.052677 -0.116960 -0.059605 -0.036022  0.065531  0.457331 -0.657879 -0.039925  0.041068  0.398628  0.140064 -0.728637 -1.168845 -1.136780 -0.543872  0.354031  0.333846 -0.034594  0.349863 -0.261857 -0.073026 -0.252984 -0.040697 -0.156456 -0.268621 -0.375567 -0.601692 -0.618719 -0.793257 -0.827946 -0.734021 -0.997266 -1.021654 -1.111002 -1.154715 -1.170184 -1.174280 -1.172648 -0.774806 -0.348025 -0.688046 -0.817432 -0.930056 -0.912449 -0.891718 -0.712229 -0.762462 -0.711455 -0.757333 -0.586552 -0.481189 -0.308510 -0.140134  0.050622 -0.018548  0.246804  0.490504  0.541491  0.860027  0.873516  0.872750  0.859426  0.501321  0.677634  0.102204 -0.093738 -0.171448  0.002883  0.051569  0.072107  0.060028  0.034715 -0.039165 -0.713681 -0.842018 -0.798933  0.392669  0.521680  0.869872  0.856952  0.873678  0.871426  0.871029  0.870842  0.857452  0.832857  0.502730  0.108408  0.384605  0.113642  0.078610  0.121702  0.181760  0.088805  0.137834 -0.034287 -0.491632 -0.032462  0.298038  0.859440  0.872584  0.863335  0.812805  0.844253  0.716383  0.635956  0.665480  0.197644  0.431795  0.233974  0.112006 -0.047559  0.292560  0.269174  0.457478  0.755109 -0.046798  0.163566  0.739221  0.725366  0.495155 -0.059985 -0.036991  0.033085  0.136027  0.613250  0.716281  0.833390  0.862377  0.869398  0.871341  0.864909  0.853724  0.762695  0.581866  0.516976  0.431652  0.352936  0.128692 -0.801020 -0.778392 -0.252774 -0.362622  0.040678  0.042043 -0.002382  0.512912  0.328397  0.058220  0.825142  0.877005  0.471250  0.085799 -0.084051 -0.206602 -0.401586 -0.628316 -0.710738 -0.626836 -0.744100 -0.789309 -0.858959 -0.855805 -0.772968 -0.707660 -0.389122 -0.456350 -0.038159  0.095138 -0.088878  0.024784  0.531023 -0.176729 -0.763871 -0.239437 -1.131035 -1.076939 -1.071865 -0.765031 -0.593413 -0.681095 -0.760457 -0.274476  0.340295 -0.215920  0.186072 -0.150858 -0.219705 -0.206012 -0.353175 -0.523212 -1.087956 -1.163575 -1.166571 -1.173404 -0.625512 -0.894094 -1.172393
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.036766  0.044953  0.035425  0.018361  0.014940  0.018792  0.011054  0.009987  0.011340  0.013531  0.065380  0.063198  0.056718  0.054265  0.062278  0.072378  0.073560  0.074249  0.073545  0.071364  0.076876  0.081416  0.078120  0.076986  0.080064  0.059544  0.062900  0.044610  0.050813  0.066161  0.074168  0.071536  0.080184  0.081020  0.078928  0.075305  0.072892  0.072526  0.022567  0.023511  0.038118  0.063932  0.039064  0.043099  0.024493  0.076116  0.076438  0.075309  0.080825  0.068043  0.074771  0.067597  0.027016  0.028172  0.045867  0.064794  0.005553 -0.005468 -0.003015  0.001904 -0.000358  0.023486 -0.001069  0.048935
        -0.070960 -0.061081 -0.047473 -0.041448 -0.047325 -0.068082 -0.223884 -0.207174 -0.194147 -0.187129 -0.078340 -0.074369 -0.071566 -0.085903 -0.105974 -0.100016 -0.105233 -0.112590 -0.110387 -0.132875 -0.133214 -0.141142 -0.129005 -0.126312 -0.128445 -0.195822 -0.187534 -0.179171 -0.159341 -0.160231 -0.159025 -0.175785 -0.173455 -0.165276 -0.160354 -0.176914 -0.150575 -0.140891 -0.171811 -0.185594 -0.164476 -0.173695 -0.192727 -0.203768 -0.220474 -0.116346 -0.123103 -0.121079 -0.146619 -0.084934 -0.115520 -0.087903 -0.086347 -0.069395 -0.095582 -0.076316 -0.027123 -0.046793 -0.208536 -0.070726 -0.192084 -0.212431 -0.051349 -0.069834
         0.191770  0.170440  0.161343  0.183442  0.193162  0.209908  0.114677  0.091351  0.075315  0.068715  0.126000  0.148300  0.165406  0.188744  0.196666  0.165543  0.158940  0.123876  0.116546  0.083662  0.097553  0.114478  0.130041  0.149440  0.169240  0.118743  0.111741  0.084610  0.074717  0.086870  0.099279  0.111710  0.140672  0.133747  0.114312  0.123470  0.085946  0.075320  0.062178  0.075478  0.068113  0.097026  0.095286  0.112402  0.115861  0.184679  0.152252  0.115774  0.108629  0.120004  0.129144  0.168529  0.220478  0.199762  0.205753  0.143906  0.183399  0.215139  0.091352  0.233826  0.068212  0.102500  0.215198  0.178406
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.612714  0.731984  0.738152  0.673971  0.673259  0.643347  0.082626  0.152840  0.397378  0.499432  0.973503  0.885326  0.778236  0.636753  0.738179  1.041770  1.059417  1.063889  1.060746  0.833385  1.003473  1.068305  1.036698  1.052387  1.040682  0.772471  0.784026  0.455555  0.589160  0.678033  0.913151  0.866622  1.055319  1.067281  0.997849  0.937113  0.872780  0.736514  0.569174  0.509538  0.589226  0.718515  0.545932  0.352044  0.592760  0.968891  1.048260  1.037379  1.048316  0.997400  1.062205  0.935602  0.676403  0.662963  0.685504  0.923134  0.625475  0.562445  0.048321  0.619882  0.125435  0.409320  0.603747  0.692434
         0.480256  0.592337  0.618811  0.524123  0.457672  0.519100 -1.050853 -0.994315 -0.916791 -0.834280  0.339755  0.486743  0.573738  0.433832  0.395579  0.136456  0.061548  0.027444  0.013027  0.356178  0.108902 -0.011665  0.154113  0.081093  0.145007 -0.808142 -0.764059 -0.769169 -0.663174 -0.705199 -0.566355 -0.649128 -0.372859 -0.318615 -0.454131 -0.593463 -0.627986 -0.253176 -0.757911 -0.767635 -0.727332 -0.696365 -0.868779 -0.943048 -0.941580  0.280159  0.090249  0.135767 -0.231580  0.282132  0.042100  0.357589  0.488444  0.512475  0.444583  0.435020  0.601770  0.547598 -1.035967  0.499438 -0.988393 -0.940955  0.523001  0.545790
         0.795463  0.488297  0.410821  0.683747  0.751367  0.724835 -0.442996 -0.508650 -0.498061 -0.518734  0.103422  0.244529  0.416393  0.813710  0.736669  0.231577  0.175400  0.092216  0.015531 -0.340781 -0.190194  0.018113  0.161227  0.074769  0.119875 -0.223017 -0.272443 -0.610391 -0.600809 -0.486386 -0.261542 -0.267478  0.043983  0.072294 -0.148900 -0.165966 -0.277728 -0.663943 -0.535908 -0.576447 -0.547901 -0.449176 -0.439539 -0.485944 -0.280472  0.398541  0.052043  0.023851 -0.129580  0.080548  0.117734  0.387859  0.718386  0.708158  0.752639  0.186202  0.652643  0.784502 -0.467864  0.775137 -0.523453 -0.463526  0.766851  0.633287
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.012884  0.021887  0.008049  0.020442  0.012411  0.007816  0.008550  0.020794  0.020305  0.014364  0.009048  0.005621  0.012686  0.006059  0.012048  0.017248  0.009957  0.048469  0.050897  0.052139  0.047758  0.051109  0.051454  0.055390  0.064743  0.069743  0.060442  0.073300  0.078117  0.079363  0.074188  0.072270  0.069041  0.063917  0.061100  0.058731  0.053558  0.050452  0.049328  0.052526  0.051033  0.054255  0.061913  0.053050  0.060874  0.049110  0.059900  0.049364  0.059250  0.051798  0.048079  0.056039  0.048864  0.064347  0.071150  0.069092  0.070371  0.067277  0.076087  0.074042  0.066298  0.077692  0.079599  0.081899  0.078661  0.080936  0.075845  0.041053  0.034631  0.039094  0.044695  0.035530  0.045182  0.036253  0.026846  0.029046  0.024516  0.039023  0.040888  0.035031  0.035772  0.036639  0.050313  0.029041  0.025102  0.026809  0.039492  0.038440  0.040923  0.043825  0.034379  0.038918  0.031592  0.022936  0.027275  0.027567  0.061603  0.062583  0.050292  0.062265  0.067783  0.043422  0.028635  0.026981  0.032960  0.057184 -0.013133 -0.005290 -0.011541 -0.018441 -0.005206 -0.012021 -0.013392 -0.005417 -0.007568  0.000022 -0.008564 -0.011941 -0.006613 -0.010624 -0.005567  0.000422  0.021878  0.001746  0.000275  0.017987  0.000704  0.001523
        -0.090738 -0.099844 -0.105832 -0.111980 -0.117478 -0.132107 -0.138713 -0.145316 -0.157274 -0.234213 -0.237215 -0.240143 -0.235110 -0.239400 -0.236511 -0.234701 -0.234296 -0.111423 -0.120594 -0.126819 -0.136664 -0.141399 -0.154123 -0.140296 -0.141269 -0.141858 -0.129774 -0.132614 -0.138836 -0.150342 -0.151430 -0.157300 -0.167379 -0.163701 -0.164439 -0.166702 -0.174382 -0.166684 -0.193143 -0.185173 -0.183111 -0.192993 -0.192841 -0.203264 -0.202342 -0.212372 -0.206062 -0.216767 -0.210499 -0.217875 -0.219300 -0.209290 -0.216398 -0.204092 -0.197692 -0.201391 -0.195640 -0.193063 -0.179824 -0.172760 -0.177977 -0.164657 -0.163834 -0.173338 -0.185741 -0.159021 -0.189932 -0.219919 -0.227011 -0.225430 -0.221217 -0.223695 -0.220507 -0.220345 -0.224743 -0.222789 -0.224965 -0.190626 -0.186054 -0.184312 -0.172892 -0.161102 -0.160812 -0.165740 -0.159050 -0.138694 -0.138574 -0.151252 -0.207189 -0.211312 -0.218357 -0.211691 -0.220178 -0.228695 -0.229979 -0.228542 -0.209076 -0.205596 -0.195602 -0.177031 -0.149406 -0.151502 -0.125625 -0.112135 -0.106916 -0.115961 -0.085430 -0.101446 -0.113307 -0.127683 -0.125790 -0.139082 -0.152969 -0.154387 -0.253308 -0.247135 -0.250890 -0.249155 -0.241317 -0.237557 -0.247731 -0.084106 -0.233227 -0.241385 -0.243482 -0.230888 -0.144522 -0.099383
         0.241099  0.240578  0.252872  0.249038  0.255292  0.261680  0.263287  0.259444  0.262514  0.194669  0.181881  0.168140  0.169203  0.160200  0.139684  0.139484  0.131931  0.220617  0.229026  0.232121  0.238627  0.233872  0.228703  0.228504  0.215716  0.209010  0.222817  0.202897  0.195252  0.188332  0.201127  0.202068  0.200899  0.215247  0.220311  0.222201  0.221720  0.227796  0.214652  0.216759  0.219903  0.201601  0.186746  0.191663  0.182738  0.187686  0.181925  0.176152  0.173545  0.161334  0.146801  0.140723  0.136439  0.149308  0.155836  0.167831  0.173297  0.179444  0.174024  0.181802  0.194968  0.181107  0.174664  0.162058  0.162740  0.175400  0.150477  0.134921  0.147942  0.153332  0.159882  0.172353  0.171882  0.186411  0.188125  0.195034  0.208412  0.230502  0.229635  0.240294  0.241644  0.243235  0.229015  0.253948  0.259156  0.254497  0.245595  0.244153  0.215232  0.196917  0.197817  0.208398  0.196082  0.174835  0.155126  0.140592  0.155800  0.178278  0.208370  0.208409  0.211336  0.238131  0.249633  0.244533  0.235937  0.213044  0.255775  0.258143  0.264459  0.269571  0.265178  0.269611  0.272396  0.270150  0.181634  0.188205  0.166921  0.154492  0.141547  0.134277  0.167143  0.246294  0.139809  0.148765  0.167697  0.194866  0.266935  0.253722
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.579835  0.574145  0.458387  0.505013  0.426811  0.376464  0.355468  0.484328  0.501953  0.668010  0.642213  0.601442  0.538642  0.467169  0.279769  0.363693  0.173324  0.718812  0.716012  0.734649  0.740072  0.800353  0.758342  0.849990  0.854797  0.875385  0.972189  0.890774  1.002809  1.029488  0.949418  0.945129  0.940288  0.920895  0.848526  0.684699  0.736043  0.641090  0.862778  0.792072  0.756630  0.865588  0.833792  0.787920  0.800965  0.623096  0.764966  0.575945  0.725299  0.599622  0.770413  0.751745  0.652246  0.803956  0.823436  0.845204  0.866278  0.832561  0.933435  0.944755  0.891336  0.983179  1.017797  1.059827  1.015052  1.039783  0.918871  0.385441  0.360321  0.525360  0.486424  0.364086  0.457170  0.477589  0.558350  0.610498  0.708382  0.837481  0.813650  0.905755  0.862537  0.825391  0.702704  0.863179  0.702224  0.527054  0.671684  0.788031  0.875066  0.719724  0.580734  0.789420  0.599223  0.505897  0.358613  0.645335  0.773070  0.797175  0.861932  0.892093  0.864893  0.778228  0.504822  0.558692  0.639462  0.742544  0.423037  0.418155  0.348730  0.254309  0.315016  0.265194  0.288511  0.284251  0.658838  0.716717  0.645393  0.309119  0.151899  0.051721  0.683061  0.543121  0.473425  0.399390  0.529131  0.669647  0.284158  0.448659
         0.410757  0.412523  0.245681  0.254387  0.183708  0.106613  0.079950  0.066675 -0.041200 -0.987951 -1.008981 -1.043150 -1.084312 -1.126450 -1.176515 -1.158161 -1.137832  0.485757  0.386959  0.202867 -0.178813 -0.384650 -0.263434 -0.335832 -0.150908 -0.132432 -0.058624  0.038046 -0.131943 -0.351615 -0.304431 -0.416383 -0.481919 -0.466154 -0.241387 -0.536348 -0.516881 -0.376495 -0.628818 -0.612926 -0.545912 -0.695987 -0.565884 -0.631628 -0.536836 -0.926481 -0.687034 -1.027636 -0.929576 -1.052476 -0.909152 -0.893983 -0.943773 -0.849930 -0.840214 -0.825319 -0.604541 -0.518667 -0.421375 -0.526163 -0.658736 -0.479030 -0.436912 -0.309023 -0.518631 -0.372711 -0.697224 -1.050019 -1.161212 -1.089176 -1.118347 -1.157841 -1.111389 -1.087526 -1.062683 -1.024447 -0.954027 -0.378242 -0.437239 -0.341644 -0.300552 -0.315453 -0.257087 -0.226602 -0.098824  0.056825 -0.099276 -0.365384 -0.730665 -0.848801 -1.040752 -0.853342 -1.020729 -1.092533 -1.173340 -1.013588 -0.903052 -0.770794 -0.707490 -0.558024 -0.176155 -0.395527  0.164970  0.302157  0.415192  0.440364  0.301340  0.213606  0.094467  0.040412  0.091042  0.019579  0.002643 -0.009940 -0.980400 -0.939916 -0.993391 -1.118530 -1.119383 -1.084188 -0.968271  0.365333 -1.118035 -1.148011 -1.079604 -0.989128  0.003409  0.263736
         0.894604  0.898218  1.073624  1.043372  1.114105  1.160074  1.175336  1.119790  1.131158  0.126658  0.122892  0.154840  0.216380  0.137395 -0.071785 -0.025430 -0.264218  0.661088  0.776759  0.884414  0.958991  0.849876  0.927531  0.803289  0.828677  0.801338  0.627360  0.752999  0.575905  0.426559  0.660000  0.615582  0.577719  0.632513  0.831899  0.908879  0.867405  1.006978  0.599989  0.736470  0.828597  0.499485  0.701568  0.717468  0.766159  0.588422  0.703028  0.431501  0.371806  0.103629  0.195136 -0.076508 -0.190447 -0.040091  0.010126  0.259281  0.619764  0.734712  0.628564  0.523803  0.491925  0.466174  0.387299  0.320949  0.273900  0.362348 -0.028304 -0.303635  0.002363  0.285578  0.154234  0.311717  0.363182  0.417104  0.312056  0.333810  0.180211  0.802572  0.812511  0.706219  0.789516  0.838253  0.980759  0.802136  0.992558  1.098017  1.019410  0.870461  0.388950  0.566739  0.356442  0.366609  0.388212  0.318127  0.171955  0.095141  0.045298  0.553678  0.489816  0.616121  0.814577  0.870991  1.079572  0.983210  0.835569  0.682920  1.068280  1.107518  1.173874  1.216857  1.187372  1.217228  1.212949  1.216314  0.016588  0.108189  0.021893 -0.221151 -0.309977 -0.394239  0.085826  0.955859  0.024824  0.007666  0.022057  0.174572  1.214443  1.071165
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.994421 0.094730 -0.046401]';
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.095661 -0.789197 -0.606644]';
q1a = [0.035447 -0.093433 0.099346]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.263141 0.607808 -0.749217]';
q1b = [0.035447 -0.093433 0.099346]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.960003 0.087962 -0.265814]';
q1c = [0.035447 -0.093433 0.099346]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [0.174284 -0.733861 -0.656562]';
q2a = [-0.001341 -0.008460 -0.005397]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.123760 0.645154 -0.753962]';
q2b = [-0.001341 -0.008460 -0.005397]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [0.976887 0.212659 0.021618]';
q2c = [-0.001341 -0.008460 -0.005397]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.125691 -0.748258 -0.651392]';
q3a = [-0.015170 0.079055 -0.108595]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [0.444563 0.629465 -0.637289]';
q3b = [-0.015170 0.079055 -0.108595]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.886886 -0.209483 0.411765]';
q3c = [-0.015170 0.079055 -0.108595]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [-0.094593 -0.674492 -0.732198]';
q4a = [0.004702 0.107194 -0.137083]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [-0.914922 0.348802 -0.203113]';
q4b = [0.004702 0.107194 -0.137083]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [0.392390 0.650691 -0.650101]';
q4c = [0.004702 0.107194 -0.137083]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.095661 0.789197 0.606644]';
q5a = [0.035447 -0.093433 0.099346]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.077190 -0.613485 0.785925]';
q5b = [0.035447 -0.093433 0.099346]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.992417 -0.028355 -0.119605]';
q5c = [0.035447 -0.093433 0.099346]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [0.003333 0.788431 0.615114]';
q6a = [0.043083 -0.154119 0.177089]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [-0.481004 -0.538019 0.692221]';
q6b = [0.043083 -0.154119 0.177089]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.876712 -0.298180 0.377446]';
q6c = [0.043083 -0.154119 0.177089]';
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
