function [new_config, new_ctrm, new_norms]  = gatsby140_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.013560  0.015110  0.015256  0.013281 -0.007103 -0.005958 -0.010596 -0.003557 -0.007014 -0.000912 -0.009738 -0.003777 -0.002913 -0.013942 -0.010371 -0.006954 -0.007165 -0.009998  0.002370 -0.010684 -0.000417 -0.005662 -0.006937  0.001091 -0.042899 -0.041335 -0.044436 -0.036983 -0.047870 -0.041071 -0.043777 -0.043008 -0.035612 -0.040786 -0.021014 -0.023147 -0.017913 -0.020055 -0.028851 -0.022558 -0.018954 -0.024080 -0.020745 -0.024137 -0.014022 -0.014524 -0.022131 -0.050424 -0.052919 -0.048070 -0.049619 -0.051524 -0.056250 -0.056651 -0.063307 -0.063266 -0.059084 -0.056076 -0.055520 -0.054516 -0.050741 -0.050577 -0.047198 -0.055040 -0.053688 -0.059563 -0.055848 -0.055803 -0.062023 -0.050461 -0.052492 -0.047794 -0.046911 -0.042615 -0.040914 -0.043736 -0.048100 -0.042268 -0.040302 -0.032635 -0.036651 -0.050016 -0.042102 -0.054320 -0.061594 -0.056439 -0.049111 -0.039110 -0.043903 -0.037069 -0.030198 -0.030703 -0.054490 -0.024372 -0.030654 -0.031515 -0.014165 -0.044382 -0.045689 -0.032195 -0.031712 -0.023587 -0.035508 -0.032371 -0.021447 -0.016876 -0.018969 -0.021654 -0.025475 -0.036905  0.007995  0.003109  0.007403  0.008163  0.006541 -0.015509 -0.001995  0.003792  0.004381  0.015244  0.002098  0.009677  0.002832  0.011386 -0.011507  0.001445  0.008887  0.021796
        -0.096683 -0.097913 -0.097724 -0.092651  0.098967  0.098356  0.091502  0.091198  0.085863  0.084938  0.071732  0.072416  0.069895 -0.067207 -0.073943 -0.084111 -0.085680 -0.089011 -0.093638 -0.094475 -0.095720 -0.094933 -0.093910 -0.094241 -0.064532 -0.067315 -0.059360 -0.063588 -0.049765 -0.044171 -0.039498 -0.032483 -0.037684 -0.025378 -0.060866 -0.059936 -0.069343 -0.073490 -0.049884  0.052140  0.056520  0.058807  0.061727  0.073456  0.075526  0.089328  0.083912  0.056224  0.042917  0.039836  0.034394  0.022975  0.023338  0.023360 -0.024316 -0.000494 -0.002124 -0.001691 -0.006589 -0.006701 -0.001313 -0.016642 -0.023476 -0.033676 -0.020442 -0.022477 -0.038403 -0.045050 -0.034500 -0.054084 -0.036790 -0.005667 -0.001365  0.005980  0.028013  0.018352  0.018272  0.034894  0.046238  0.045089  0.037048  0.007688  0.010855 -0.013912 -0.021257  0.010854  0.028223  0.052760  0.055740  0.067819  0.077543  0.065445  0.035707  0.080575  0.060121  0.048488  0.061084 -0.025407 -0.039877 -0.063199 -0.076721 -0.083929 -0.072425 -0.078256 -0.085278 -0.089782 -0.090787 -0.084852 -0.083739 -0.069954 -0.095776 -0.094850 -0.092843 -0.093847 -0.094094 -0.087238 -0.085162  0.075970  0.079214  0.089276  0.106036  0.107796  0.092130  0.090542  0.077179  0.103297 -0.095588  0.089563
        -0.031540 -0.037767 -0.050360 -0.088051 -0.095348 -0.083820 -0.071883 -0.059399 -0.056964 -0.046475 -0.041528 -0.029757 -0.015315 -0.032173 -0.036764 -0.051235 -0.055471 -0.062068 -0.073937 -0.070888 -0.093498 -0.096904 -0.106313 -0.119785 -0.096766 -0.087085 -0.076498 -0.065824 -0.067397 -0.053855 -0.053318 -0.040867 -0.029407 -0.023719 -0.038000 -0.041853 -0.046958 -0.054834 -0.036059 -0.015266 -0.021965 -0.045241 -0.044286 -0.065810 -0.053362 -0.078280 -0.084875 -0.084143 -0.073364 -0.061143 -0.060307 -0.058135 -0.070327 -0.082691 -0.085356 -0.088502 -0.081895 -0.070458 -0.059930 -0.048057 -0.040763 -0.039549 -0.037796 -0.065966 -0.059746 -0.074693 -0.070921 -0.084192 -0.092030 -0.082992 -0.062763 -0.034014 -0.031573 -0.022673 -0.027937 -0.034692 -0.044996 -0.045733 -0.054600 -0.033783 -0.017028 -0.044673 -0.017055 -0.054668 -0.079830 -0.078725 -0.054880 -0.060621 -0.071622 -0.078782 -0.089041 -0.064568 -0.071296 -0.078734 -0.057568 -0.040673 -0.021803 -0.033620 -0.056140 -0.058340 -0.078228 -0.071511 -0.079483 -0.095421 -0.111653 -0.104471 -0.083468 -0.068148 -0.077106 -0.078051 -0.107531 -0.080097 -0.076243 -0.055054 -0.041016 -0.062353 -0.037992 -0.015533 -0.026462 -0.027680 -0.087994 -0.065316 -0.050460 -0.036304 -0.051864 -0.077536 -0.111531 -0.009158
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.378459 -0.279563 -0.283425  0.295650 -0.704256 -0.662415 -0.637531 -0.670520 -0.674533 -0.669546 -0.685866 -0.694966 -0.645072 -0.771851 -0.768114 -0.630116 -0.472580 -0.176551  0.272513 -0.309550 -0.049506 -0.186519 -0.277889 -0.124826 -0.825358 -0.825563 -0.815469 -0.775376 -0.822394 -0.743175 -0.784004 -0.849617 -0.824960 -0.929699 -0.778979 -0.779249 -0.751293 -0.628741 -0.790579 -0.713379 -0.698605 -0.709365 -0.689215 -0.682091 -0.673146 -0.737894 -0.664786 -0.812419 -0.900586 -0.857564 -0.924727 -0.922326 -1.044857 -1.056479 -0.989213 -0.842131 -0.902946 -1.053326 -1.051255 -1.035400 -0.907936 -0.934085 -0.886020 -0.925370 -1.012447 -0.977707 -0.955821 -0.931146 -0.963597 -0.864761 -0.842920 -0.964145 -0.979312 -1.024540 -1.018446 -0.965524 -0.966135 -0.885470 -0.781920 -0.791598 -0.852794 -0.955874 -1.042031 -1.033148 -0.987445 -1.054561 -0.963139 -0.736019 -0.740243 -0.710557 -0.699081 -0.703137 -0.957320 -0.680015 -0.696971 -0.760351 -0.702473 -0.890613 -0.767449 -0.655456 -0.785635 -0.767305 -0.798299 -0.785134 -0.642318 -0.515827 -0.581180 -0.752024 -0.750098 -0.799983 -0.018703  0.245651  0.164936 -0.460801 -0.554604 -0.444223 -0.740260 -0.666642 -0.670629 -0.535052 -0.658292 -0.579829 -0.665494 -0.545978 -0.673623 -0.645025 -0.031879 -0.536893
        -1.008023 -1.048727 -1.049054 -1.047133  0.770395  0.843345  0.802087  0.726954  0.702228  0.683182  0.698963  0.765817  0.816619 -0.657877 -0.664642 -0.819819 -0.887208 -0.804504 -1.026090 -1.046886 -1.094303 -1.075327 -1.051650 -1.083556 -0.663479 -0.682736 -0.625528 -0.631146 -0.588619 -0.516312 -0.507366 -0.547697 -0.588544 -0.378361 -0.651371 -0.649383 -0.653671 -0.571689 -0.636332  0.760258  0.779207  0.687093  0.703532  0.695770  0.676578  0.767763  0.829164  0.649766  0.392681  0.423887  0.303423  0.257191  0.088222  0.006756 -0.198198  0.286153  0.248701 -0.007316 -0.085467 -0.049333  0.200807 -0.243648 -0.418894 -0.208103 -0.169193  0.021189 -0.399015 -0.479995 -0.405306 -0.580336 -0.299667  0.030226  0.082259  0.086564  0.236405  0.245127  0.245429  0.426883  0.558711  0.681344  0.601401  0.234285  0.015691 -0.176581 -0.031977  0.030039  0.250105  0.612625  0.628146  0.736664  0.797151  0.662000  0.280217  0.794103  0.656094  0.682946  0.779787 -0.413128 -0.452034 -0.557895 -0.684399 -0.707482 -0.667299 -0.724859 -0.862153 -0.948116 -0.915972 -0.693001 -0.744820 -0.656546 -1.094861 -1.057447 -1.073472 -0.963223 -0.918393 -0.674071 -0.715599  0.801113  0.777987  0.873593  0.828475  0.856315  0.697961  0.810440  0.678147  0.843445 -1.091923  0.915257
         0.094083  0.057738 -0.126259  0.008037  0.155972  0.034735  0.247526  0.329156  0.360782  0.395149  0.345037  0.173840  0.157990  0.244746  0.240545  0.214098  0.319951  0.616407  0.165975 -0.010947 -0.114036 -0.175118 -0.202855 -0.193270 -0.267122 -0.008071  0.214984  0.298394  0.263886  0.479185  0.429552  0.248851  0.241672  0.268233  0.239116  0.242508  0.300483  0.562660  0.239349  0.130793  0.103725  0.323054  0.331527  0.358244  0.398625  0.050022  0.092871  0.174841  0.325541  0.382956  0.341949  0.374124  0.065487 -0.074384  0.249999  0.469354  0.392429  0.003449 -0.012082  0.141810  0.432909  0.370185  0.336400  0.394577  0.182439  0.319141  0.174045  0.126988  0.130832  0.161907  0.490517  0.364748  0.320044  0.148581  0.003738  0.279757  0.279984  0.322174  0.378268  0.104617  0.107127  0.313203 -0.006142  0.069571  0.297907 -0.020540  0.284710  0.387563  0.361346  0.235959  0.097399  0.373560  0.282416  0.168268  0.390583  0.216739  0.076678  0.331848  0.499907  0.547318  0.171425  0.169459  0.174041 -0.218922 -0.216458 -0.224096 -0.028794  0.242841  0.117559  0.193282 -0.140923  0.040656  0.040160 -0.210807  0.034133  0.624547  0.201830  0.141586  0.210345  0.258713  0.116862  0.238334  0.381177  0.377330  0.395842  0.107552 -0.186733  0.068923
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.018360  0.025337  0.014583  0.024443  0.020638  0.015443  0.013511  0.016627  0.024755 -0.008710 -0.005560 -0.006249 -0.000011 -0.001462  0.000065 -0.012210 -0.005139 -0.006761 -0.003875 -0.037249 -0.036242 -0.037712 -0.039409 -0.016441 -0.018764 -0.017223 -0.025654 -0.033343 -0.028725 -0.034773 -0.025797 -0.022961 -0.019534 -0.017884 -0.014921 -0.012604 -0.041678 -0.041427 -0.041277 -0.038863 -0.035780 -0.041002 -0.036474 -0.041530 -0.011207 -0.032686 -0.037359 -0.041109  0.007405 -0.001232  0.006208  0.011154  0.006645  0.011881  0.010267 -0.001838  0.008585  0.007171 -0.007953  0.003893  0.006854  0.014684  0.023965  0.015172
         0.078201  0.075317  0.059875  0.055712  0.042217 -0.097532 -0.094011 -0.097798 -0.099955  0.057623  0.053129  0.047977  0.039038  0.027628  0.023849 -0.064742 -0.069341 -0.071264 -0.069425 -0.030360 -0.027146 -0.030122 -0.020487 -0.057365 -0.051633 -0.056261 -0.046123 -0.036658 -0.037227 -0.036345 -0.046306  0.005316  0.010850  0.022425  0.040526  0.051490 -0.006668 -0.009152 -0.006949  0.008384  0.016731  0.022508  0.028187  0.010813  0.033455 -0.001324 -0.014377 -0.013774 -0.082283 -0.078032 -0.082237 -0.089678 -0.089048 -0.090282 -0.085583 -0.069857  0.028970  0.035042  0.036369  0.063092  0.068142 -0.094147 -0.109013  0.076916
         0.024323  0.052063  0.058486  0.078430  0.090687  0.056787  0.053047  0.023079  0.008415  0.016432  0.035848  0.042787  0.063930  0.076766  0.084482  0.065762  0.033712  0.023084  0.002317  0.017174  0.034430  0.055599  0.071009  0.057167  0.033900  0.027197  0.016170  0.018604  0.038693  0.063200  0.022572  0.074522  0.071968  0.059059  0.039964  0.026880  0.014476  0.034315  0.051901  0.044681  0.039882  0.016550  0.022594  0.028671  0.054632  0.068255  0.077944  0.040880 -0.003741  0.023562  0.041341  0.048724  0.071067  0.035724 -0.000629  0.047240  0.090531  0.079705  0.055661  0.035625  0.028861  0.033264  0.072967  0.022391
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.437652 -0.484921 -0.654315 -0.644969 -0.655105 -0.665062 -0.694252 -0.516882 -0.396053 -0.689045 -0.672075 -0.678721 -0.669897 -0.661726 -0.640124 -0.794954 -0.767502 -0.787296 -0.769851 -0.938581 -0.920782 -0.913083 -1.022767 -0.775667 -0.780807 -0.802714 -0.805947 -0.839809 -0.811208 -0.858085 -0.813124 -0.715639 -0.699814 -0.687790 -0.682724 -0.692400 -1.040229 -1.036164 -1.032970 -0.888241 -0.754393 -1.005934 -0.776736 -1.026428 -0.680862 -0.716088 -0.935184 -1.025971 -0.734557 -0.785681 -0.795525 -0.727700 -0.752202 -0.724498 -0.717754 -0.758280 -0.678099 -0.662961 -0.678309 -0.688110 -0.674907 -0.647113 -0.721900 -0.542339
         0.946720  0.850689  0.652580  0.644305  0.652755 -0.787026 -0.750177 -0.940040 -0.988214  0.785851  0.703581  0.657744  0.626670  0.636945  0.665710 -0.686759 -0.656847 -0.690617 -0.688625 -0.461229 -0.487910 -0.488516 -0.150782 -0.667009 -0.647938 -0.646000 -0.664079 -0.627041 -0.656220 -0.603934 -0.659826  0.616037  0.616057  0.610044  0.637942  0.719852 -0.060117 -0.111412  0.079601  0.425072  0.570745  0.275028  0.615415  0.145051  0.621684  0.592492  0.341906 -0.174453 -0.752720 -0.692228 -0.678630 -0.733130 -0.701681 -0.741448 -0.759663 -0.695471  0.652193  0.628524  0.624225  0.727696  0.774142 -0.818308 -0.773337  0.886117
         0.218560  0.424132  0.507125  0.531693  0.503744 -0.191226 -0.208711  0.100521 -0.175749  0.099077  0.400087  0.462271  0.517737  0.515837  0.506586  0.045576  0.305078  0.107732 -0.170518 -0.027023  0.107134 -0.098025  0.197618 -0.170671  0.286309  0.214848 -0.080122 -0.013952  0.106282  0.063590  0.083486  0.458209  0.485419  0.512852  0.483963  0.328824  0.020739  0.027865  0.147052  0.368607  0.460197  0.088245  0.349970  0.138012  0.508007  0.492927  0.336735 -0.024162 -0.001422  0.109604  0.080579 -0.151436  0.292046  0.219381 -0.125537 -0.143551  0.464776  0.525464  0.508483  0.317465  0.220604  0.227211  0.121523  0.207799
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.038756  0.039812  0.050777  0.039311  0.050582  0.044478  0.049978  0.037913  0.041381  0.049227  0.052611  0.045122  0.039709  0.051664  0.067595  0.073635  0.069726  0.068068  0.074010  0.067894  0.062064  0.077651  0.075704  0.074509  0.071555  0.074972  0.065530  0.117774  0.113095  0.118556  0.114593  0.123524  0.111773  0.124112  0.111493  0.137299  0.137678  0.133730  0.126715  0.127016  0.125493  0.133934  0.093897  0.102813  0.089450  0.098619  0.090429  0.099344  0.102681  0.087577  0.091230  0.095737  0.087447  0.101231  0.098638  0.082810  0.108361  0.082940  0.081890  0.130152  0.106538  0.106569  0.107068  0.078947  0.085063  0.061137  0.057662  0.080755  0.055371  0.080527  0.015993  0.022183  0.014120  0.018773  0.009940  0.021823  0.017783  0.020295  0.024992  0.026926  0.012543  0.019673  0.023805  0.014184  0.018361  0.024492 -0.012706 -0.005218 -0.001233 -0.006663 -0.011029 -0.014459  0.000655 -0.013614 -0.011090 -0.009731  0.002207 -0.000983  0.000910 -0.013704 -0.005080 -0.010988 -0.007667 -0.014326 -0.005171 -0.014433 -0.035441 -0.031357 -0.023454 -0.024720 -0.033533 -0.038634 -0.034996 -0.034260 -0.039633 -0.029806 -0.029774 -0.032479 -0.033681 -0.034117 -0.032210 -0.026283 -0.031065 -0.035468 -0.030281 -0.039105 -0.036693 -0.039196 -0.031728 -0.025256 -0.030436 -0.037092 -0.025951 -0.019969 -0.036225 -0.048462 -0.056635 -0.054011 -0.058057 -0.062755 -0.050573 -0.055137 -0.060098 -0.071124 -0.064992 -0.061015 -0.068343 -0.068746 -0.072485 -0.070439 -0.065035 -0.059141 -0.074912 -0.068745 -0.074514 -0.050593 -0.055017 -0.088832 -0.088717 -0.098334 -0.096721 -0.100169 -0.100643 -0.091457 -0.088079 -0.098466 -0.089146 -0.085239 -0.090601 -0.087831 -0.093936 -0.101077 -0.110701 -0.120183 -0.111166 -0.112695 -0.111948 -0.109732 -0.113318 -0.131395 -0.105943 -0.078848 -0.072368 -0.080440 -0.076179 -0.068295 -0.104406 -0.075220 -0.079942 -0.106277 -0.093800 -0.049323 -0.050525 -0.043111 -0.039366 -0.083719 -0.084151 -0.056135 -0.055566 -0.057134 -0.055928 -0.050430 -0.049109 -0.055490 -0.050338 -0.049050 -0.043359 -0.044991 -0.040420 -0.043041 -0.051563 -0.068237 -0.048709 -0.066321 -0.012208 -0.013780 -0.038826 -0.041516 -0.020538 -0.019601 -0.013380 -0.018800 -0.021183 -0.027832 -0.031656 -0.022456 -0.028043 -0.019416 -0.044108 -0.018568 -0.000902 -0.020493 -0.017276 -0.015059  0.002736  0.005905 -0.013522 -0.000106 -0.018167  0.006374  0.006760  0.028493  0.032603  0.024707  0.029279  0.027275  0.030509  0.056756  0.031289  0.031503  0.029033  0.055124  0.033873  0.033227  0.053499  0.036187
         0.030142  0.016560  0.003602 -0.014452 -0.026149 -0.037688 -0.051343 -0.063996 -0.072196 -0.079645 -0.086374 -0.098514 -0.098429 -0.099724 -0.098171 -0.091622 -0.082790 -0.078129 -0.072689 -0.062663 -0.048121 -0.045182 -0.026518 -0.020240 -0.002243  0.011891  0.027071  0.001090 -0.005509 -0.025521 -0.042904 -0.053593 -0.059897 -0.060771 -0.063593 -0.033031 -0.050461 -0.058680 -0.049818 -0.032593 -0.008089 -0.025330 -0.079526 -0.070689 -0.069844 -0.062845 -0.059682 -0.052772 -0.049662 -0.047502 -0.027657 -0.009205 -0.000449  0.005781  0.020369  0.016128 -0.057656 -0.054810 -0.081300 -0.062058 -0.062580 -0.026373  0.004372  0.020244  0.012420 -0.003031 -0.020817 -0.038057 -0.064151 -0.073941  0.038877  0.017416  0.005615 -0.009799 -0.020367 -0.043356 -0.051196 -0.057098 -0.065037 -0.071896 -0.081152 -0.086401 -0.093656 -0.108155 -0.110116 -0.109846  0.019763  0.029231  0.036003  0.034906  0.024921  0.015667 -0.008696 -0.015433 -0.043280 -0.056723 -0.053012 -0.058787 -0.063941 -0.078331 -0.091402 -0.106235 -0.098020 -0.112478 -0.115241 -0.107644  0.005210  0.006317  0.014830  0.023182  0.032947  0.035043  0.029084  0.026173  0.020520  0.006447 -0.006721 -0.027474 -0.044028 -0.060270 -0.051806 -0.063726 -0.071985 -0.082061 -0.081709 -0.091879 -0.098189 -0.102348 -0.112357 -0.120881 -0.118090 -0.108032 -0.101251 -0.087434 -0.064879 -0.082784 -0.074130 -0.101003 -0.105952 -0.104910 -0.098022 -0.096967 -0.096450  0.027614  0.030563  0.031887  0.036375  0.030740  0.018020  0.026220  0.011141  0.002685 -0.016064 -0.020697 -0.027108 -0.038291 -0.057942 -0.038699 -0.029142 -0.015487 -0.006620  0.001879  0.009725  0.016805  0.024866  0.035061  0.040042  0.033395 -0.099213 -0.088082 -0.056666 -0.045986 -0.026912 -0.016388 -0.004793  0.002218  0.010439  0.031122 -0.052835 -0.020712  0.040151 -0.071101 -0.088507 -0.103061 -0.108378 -0.106902  0.032248  0.027407  0.015949 -0.005687 -0.039650 -0.010131 -0.008182  0.001927  0.005550 -0.020531 -0.005481  0.014801  0.025529  0.034111  0.038453  0.033907  0.029311  0.028544  0.020813  0.014839 -0.072018 -0.085392 -0.085975 -0.090390 -0.094241 -0.103154 -0.110502 -0.100709 -0.101515 -0.119988 -0.118971 -0.111854 -0.077337 -0.070611 -0.060813 -0.041581 -0.025892 -0.015217 -0.012541  0.009562  0.030453  0.032071  0.021687  0.018405 -0.118901 -0.111996 -0.091566 -0.071256 -0.025986 -0.007833  0.028933  0.040201  0.023902 -0.115638 -0.106053 -0.081497 -0.069541 -0.025407  0.007573  0.017109  0.037737 -0.099427 -0.088082 -0.075296 -0.053520 -0.038092 -0.027615 -0.006006  0.019652  0.037073
         0.205078  0.214895  0.226344  0.234472  0.245390  0.249850  0.252124  0.239538  0.235118  0.230759  0.224621  0.206054  0.200688  0.189365  0.206633  0.212040  0.228847  0.234474  0.245141  0.253232  0.256109  0.263536  0.259180  0.255433  0.243901  0.234603  0.215633  0.240257  0.257055  0.265261  0.272417  0.263061  0.269192  0.251811  0.222587  0.235886  0.236115  0.240023  0.259074  0.259158  0.248040  0.239974  0.210710  0.214883  0.239476  0.247296  0.260903  0.272416  0.273884  0.267998  0.268423  0.259539  0.247741  0.238600  0.223940  0.234491  0.272456  0.262533  0.223926  0.233048  0.253637  0.271081  0.237283  0.228755  0.238014  0.239786  0.246558  0.265006  0.246334  0.240731  0.198040  0.212162  0.228591  0.234529  0.249878  0.249659  0.246720  0.241239  0.235416  0.228768  0.220398  0.214799  0.207286  0.192162  0.181662  0.175931  0.187968  0.198902  0.204189  0.222446  0.239541  0.252181  0.251517  0.273589  0.276185  0.263936  0.253975  0.250547  0.240503  0.231484  0.221811  0.213020  0.219079  0.209582  0.192588  0.173040  0.169836  0.181980  0.187634  0.197090  0.206636  0.223687  0.239915  0.250754  0.274844  0.283895  0.296222  0.310390  0.305746  0.291376  0.296861  0.278723  0.276921  0.248268  0.237199  0.225759  0.219685  0.218069  0.215222  0.205678  0.187193  0.183970  0.175740  0.160441  0.160798  0.177134  0.176793  0.191533  0.208313  0.224753  0.227170  0.247631  0.271406  0.271180  0.260197  0.254550  0.221997  0.212910  0.199680  0.208794  0.192745  0.185944  0.173458  0.167600  0.166900  0.154716  0.168241  0.176532  0.173026  0.177574  0.183438  0.195108  0.203864  0.207693  0.215982  0.227094  0.236461  0.261390  0.215691  0.203758  0.189472  0.187640  0.183625  0.187104  0.191280  0.201945  0.209426  0.225651  0.198700  0.192958  0.231258  0.187596  0.194879  0.217376  0.235963  0.252396  0.226131  0.212774  0.200235  0.187049  0.179701  0.160694  0.173340  0.181931  0.164399  0.172390  0.182719  0.194489  0.202735  0.211666  0.227121  0.242231  0.259997  0.265169  0.286719  0.302436  0.291690  0.277764  0.259988  0.234398  0.247751  0.209438  0.208839  0.203475  0.165565  0.181816  0.197549  0.212873  0.235662  0.254078  0.264462  0.286649  0.292156  0.298974  0.303489  0.268010  0.223810  0.216626  0.198408  0.191102  0.171699  0.178319  0.224778  0.238046  0.259696  0.245606  0.235003  0.210185  0.196271  0.172680  0.199980  0.220541  0.233555  0.239076  0.217259  0.210103  0.197436  0.206927  0.215689  0.227708  0.244354  0.252676  0.241013  0.227664  0.216438  0.199881
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.063366 -0.219916 -0.302359 -0.203973 -0.267971 -0.139291 -0.206450 -0.235233 -0.206692 -0.081821  0.003238 -0.106802 -0.017139  0.099356  0.365324  0.464968  0.217770  0.077826 -0.002487 -0.145939 -0.286071 -0.344376 -0.222718 -0.174929 -0.147559 -0.149210 -0.150481  0.446025  0.424283  0.680584  0.552874  0.909297  0.290427  0.567389  0.374307  1.073473  1.096548  0.852614  0.984096  0.955083  0.890870  1.052486  0.630877  0.679753  0.602304  0.191509 -0.123450 -0.215447 -0.044693 -0.390226 -0.311123 -0.324851 -0.091283  0.414825  0.707166 -0.265724  0.184254 -0.280752  0.539806  0.432934  0.070635  0.230222  0.393437 -0.259421 -0.041804 -0.589885 -0.576437 -0.306190 -0.260660  0.298663  0.140656  0.575456  0.639903  0.598814  0.666006  0.318358  0.185580  0.104391 -0.102388 -0.204729  0.238831  0.082999 -0.008206  0.250199  0.189618  0.218061 -0.670521 -0.292200 -0.440259 -0.014423  0.406898  0.599693  0.666044  0.761350  0.727973  0.661057  0.524450  0.515363  0.307645  0.274931  0.377078  0.402300  0.349059  0.434692  0.476673 -0.525441 -0.619005 -0.605026 -0.296875 -0.192997  0.009963  0.289996  0.318058  0.291997  0.391910  0.722806  0.773946  0.786151  0.697975  0.650392  0.661828  0.641727  0.620877  0.461345  0.440036  0.270703  0.073117 -0.031533 -0.069853  0.084629 -0.473507 -0.478278 -0.432997 -0.543252 -0.508260 -0.419348 -0.434828 -0.603311 -0.346612  0.256079  0.407178  0.555533  0.408053 -0.023693 -0.024626  0.011137 -0.292847 -0.392682 -0.252553 -0.386642 -0.167610 -0.167044 -0.257208 -0.461563 -0.556038 -0.493678 -0.500614 -0.507093 -0.468404 -0.337404 -0.214403 -0.378697 -0.429668 -0.424378 -0.335526 -0.185537  0.191682  0.245309 -0.475634 -0.525920 -0.547124 -0.554049 -0.557495 -0.603576 -0.414433 -0.388015 -0.357922 -0.162281 -0.580749 -0.551637 -0.179575 -0.573308 -0.528233 -0.274288 -0.220524  0.443186 -0.198125 -0.416324 -0.381320 -0.320557 -0.539011 -0.760123 -0.554675 -0.297267 -0.865529 -0.255912 -0.173448 -0.127462 -0.241802 -0.278205  0.008211  0.153723  0.178460  0.068298  0.188548  0.322744  0.612528  0.493223  0.457983  0.403095  0.550788 -0.366593 -0.611192 -0.469474 -0.614370 -0.187014 -0.537599 -0.429106  0.367193  0.583466  0.667148  0.763212  0.869031  0.775187  0.781800  0.698847  0.239005 -0.140828 -0.134992 -0.365764 -0.189332 -0.405313  0.070804  0.496765  0.686333  0.649467  0.013134 -0.274106 -0.214071  0.495984  0.372614 -0.241930 -0.286420  0.374104 -0.009206  0.079788 -0.042023  0.059320 -0.268681 -0.282372 -0.108433 -0.280131 -0.268888 -0.104613 -0.296672 -0.186426
         0.621532  0.561421  0.490131  0.508032  0.468389  0.277848 -0.254174 -0.607143 -0.658389 -0.758468 -0.793595 -1.073670 -1.070617 -1.095536 -0.977750 -0.886620 -0.840515 -0.866074 -0.987201 -0.570265 -0.133564 -0.203580  0.389055  0.454380  0.532090  0.626073  0.872379  0.935369  0.671260  0.311965 -0.022340 -0.251639 -0.887735 -0.914353 -1.019899  0.098513 -0.116738 -0.679676 -0.152175  0.113102  0.452872  0.182469 -0.860628 -0.792691 -0.833383 -1.048431 -0.914675 -0.319501 -0.145958 -0.235526  0.282985  0.661858  0.639073  0.790438  0.644487  0.593856 -0.590911 -0.557566 -0.827869 -1.011585 -1.061149  0.360950  0.869471  0.705082  0.528675  0.419632  0.364334  0.080313 -0.728598 -0.999926  0.914312  0.482001  0.444520  0.384206  0.345996 -0.052800 -0.738222 -0.670581 -0.746645 -0.735596 -0.676862 -0.701206 -0.882326 -0.983695 -1.082195 -1.080612  0.588182  0.849607  0.834526  0.924584  0.722005  0.558958  0.400294  0.324390 -0.308459 -0.598286 -0.691762 -0.745537 -0.890358 -0.643744 -0.535036 -0.677715 -0.694690 -0.769709 -0.943970 -0.559483  0.690376  0.737848  0.925412  0.660602  0.857684  0.957093  0.931329  0.941298  0.870361  0.508281  0.357242  0.021087 -0.487434 -0.548216 -0.540707 -0.558707 -0.641530 -0.949899 -0.865370 -0.900043 -0.528787 -0.583507 -0.476955 -1.096102 -0.767363 -0.578385 -0.495141 -0.586962 -0.487414 -0.466164 -0.462559 -0.692684 -1.063346 -1.048788 -0.912908 -0.881636 -0.958302  0.989269  0.993235  0.995931  0.935114  0.772542  0.769389  0.694722  0.595708  0.629041  0.602179  0.571194  0.155575 -0.438878 -0.479000 -0.439010 -0.189977  0.342074  0.697358  0.746626  0.690917  0.734981  0.783332  0.595356  0.994517  0.910491 -0.853259 -0.613295 -0.479546 -0.487570 -0.315823  0.087528  0.794214  0.770420  0.601584  0.642551 -0.489074 -0.065918  0.686140 -0.490818 -0.641158 -1.019984 -1.091157 -0.960598  0.572998  0.734175  0.679086  0.809353 -0.464185  0.673727  0.687123  0.836942  0.541459  0.373920  0.642402  0.612176  0.595369  0.834908  1.029769  0.977009  0.956974  0.968108  0.955144  0.806902 -0.650116 -0.849119 -0.952099 -0.963009 -0.887184 -0.992557 -0.733886 -0.874448 -0.665828 -1.024039 -0.963697 -0.579717 -0.833911 -0.853376 -0.632078 -0.263989  0.080159  0.293056  0.287616  0.512336  0.970210  1.007303  0.593344  0.702333 -0.997982 -0.539093 -0.517716 -0.806909  0.271562  0.413992  0.925063  0.956932  0.785463 -0.964440 -0.863352 -0.761236 -0.718636  0.525582  0.619772  0.514714  0.746676 -1.076997 -0.737556 -0.699055 -0.678521  0.186249  0.498958  0.518378  0.642052  0.609192
         1.004970  1.012184  1.025459  1.051677  1.054066  1.180799  1.197166  1.033504  1.006374  0.953273  0.929888  0.367268  0.427887  0.267363  0.577710  0.675544  0.864407  0.850143  0.642402  1.087868  1.189651  1.156175  1.118391  1.099540  1.060281  0.991256  0.704298  0.379447  0.924247  1.020395  1.171020  0.849012  0.800950  0.525497  0.350683  0.561886  0.512113  0.552384  0.763475  0.800525  0.697165  0.576249  0.529351  0.596790  0.631891  0.388687  0.733389  1.183329  1.258266  1.126107  1.129593  0.870097  0.995449  0.773364  0.714011  0.972566  1.114999  1.046730  0.706573  0.315031  0.293285  1.184695  0.649104  0.874127  1.085000  0.870282  0.917063  1.179480  0.923559  0.548899  0.665022  0.903574  0.873383  0.947490  0.909693  1.204634  0.953714  1.026457  0.955961  0.939991  0.991062  1.000931  0.821269  0.613740  0.342077  0.317863 -0.306177 -0.295694 -0.207658  0.678970  0.822132  0.824590  0.876005  0.806460  0.895431  0.770645  0.819006  0.762174  0.712677  1.004140  1.032455  0.921965  0.937919  0.814917  0.477255 -0.558201  0.605726 -0.186484 -0.122337 -0.554827 -0.357206  0.428198  0.487516  0.487612  0.577400  0.710204  0.766928  0.882654  0.823501  0.833000  0.825337  0.834382  0.777510  0.254150  0.643002  0.726649  1.115524  1.094669  1.145607  0.397692 -0.430976 -0.580310 -0.648374 -0.529954 -0.612925 -0.668717 -0.663500 -0.398537  0.196795  0.311008  0.582733  0.354292  0.466458  0.541308  0.527840  0.510311 -0.154653 -0.374282 -0.435158 -0.464349 -0.612090 -0.586089 -0.591097 -0.536218 -0.690282 -0.647285 -0.621793 -0.638826 -0.738001 -0.725198 -0.516966 -0.411336 -0.450139 -0.403360 -0.386927 -0.596610  0.135898  0.609872 -0.316439 -0.525783 -0.595253 -0.586840 -0.663078 -0.682110 -0.327966 -0.376038 -0.558718 -0.563902 -0.570198 -0.713327 -0.528529 -0.573553 -0.502806 -0.131445  0.120600  0.350326 -0.609814 -0.407005 -0.484907 -0.351277 -0.608688 -0.003933 -0.338895 -0.306080  0.338344 -0.730249 -0.573768 -0.604295 -0.597639 -0.344978  0.341926  0.504679  0.556062  0.584381  0.551219  0.786591  0.777091  0.634200  0.222260  0.433137  0.339479 -0.136895  0.751982 -0.285191 -0.399193 -0.208929  0.003483  1.007152  0.762218  0.407665  0.723306  0.868880  0.735181  0.809002  0.801680  0.744393  0.437217  0.401506 -0.617035 -0.450534 -0.266149 -0.639912  1.125133  0.681727  0.926150  0.884270  0.677993 -0.080555 -0.421505  0.372220  0.740951  0.902322  0.924135  0.996923  1.007025  1.075916  0.898967  0.417519  0.914403  0.943534  1.012780  1.167704  1.035403  1.067915  0.916436  0.989262
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.011682 -0.009693 -0.010504 -0.005298 -0.006097 -0.049834 -0.041969 -0.040830 -0.019535 -0.025672 -0.014204 -0.037341 -0.054338 -0.048045 -0.051472 -0.055895 -0.072695 -0.068748 -0.074261 -0.069476 -0.071726 -0.061028 -0.068077 -0.081875 -0.083740 -0.086647 -0.086666 -0.085224 -0.080531 -0.085750 -0.083844 -0.087161 -0.082873 -0.075971 -0.074231 -0.078339 -0.081841 -0.090430 -0.088481 -0.085282 -0.079393 -0.056971 -0.056299 -0.063283 -0.056634 -0.074444 -0.066998 -0.082385 -0.078426 -0.031792 -0.042484 -0.038844 -0.035975 -0.044515 -0.030456 -0.012088 -0.032024 -0.031836 -0.029403 -0.023905 -0.024819 -0.032397 -0.017036 -0.017523
         0.133169  0.109642 -0.090210 -0.094703 -0.091140 -0.071779 -0.068579 -0.061791  0.103788  0.111490  0.126581  0.124312  0.104085  0.097554  0.081039  0.065297  0.045246  0.060741  0.071343  0.085592 -0.042637 -0.046475 -0.026447  0.010619  0.003280  0.013379  0.037902  0.043398  0.035361  0.048305  0.065668 -0.001505 -0.016099 -0.015267 -0.024478 -0.024107 -0.018929  0.010537  0.039251  0.000902  0.030580 -0.040493 -0.046469 -0.054611 -0.065196 -0.018993 -0.051078  0.060142  0.046942  0.091128  0.084480  0.093842  0.110695  0.116393  0.110071  0.112472 -0.072125 -0.085491 -0.091855 -0.094789 -0.093543 -0.076105 -0.092091  0.132824
        -0.208180 -0.116721 -0.172197 -0.215864 -0.239292 -0.207899 -0.179612 -0.161519 -0.126125 -0.164846 -0.171772 -0.204065 -0.196729 -0.179065 -0.152795 -0.127009 -0.133133 -0.144643 -0.182698 -0.194399 -0.205733 -0.184025 -0.159711 -0.128200 -0.163067 -0.187527 -0.201389 -0.170228 -0.141278 -0.180640 -0.200449 -0.208927 -0.182014 -0.157761 -0.172957 -0.186718 -0.209405 -0.208903 -0.210428 -0.176740 -0.135231 -0.145093 -0.171219 -0.207651 -0.220443 -0.141162 -0.210688 -0.181810 -0.145773 -0.125981 -0.132481 -0.158963 -0.178909 -0.200970 -0.170208 -0.129584 -0.157818 -0.189474 -0.211723 -0.207372 -0.192306 -0.169995 -0.190753 -0.208238
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.097703 -0.671322 -0.215415  0.276115 -0.133164 -0.807783 -0.794836 -0.810522 -0.707825 -0.682422 -0.569003 -0.649710 -0.709297 -0.714595 -0.697428 -0.816585 -0.805050 -0.847170 -0.747313 -0.742648 -0.875978 -0.815411 -0.839746 -0.995081 -1.063856 -1.019131 -1.056145 -1.063577 -0.973461 -1.076158 -1.062260 -1.006111 -0.918645 -0.918608 -0.840443 -0.918266 -1.060430 -1.085391 -1.057722 -1.002212 -0.991600 -0.847527 -0.811883 -0.829855 -0.836928 -0.886464 -0.860431 -0.922793 -0.964729 -0.706655 -0.747009 -0.672300 -0.689518 -0.717176 -0.690819 -0.715532 -0.802443 -0.707030 -0.531723 -0.340538 -0.407276 -0.769602  0.147573 -0.139825
         1.127616  0.762841 -1.068276 -1.044077 -1.009678 -0.682599 -0.665682 -0.671244  0.779945  0.738627  0.890856  0.888211  0.720194  0.698667  0.819046  0.569699  0.502507  0.629920  0.668635  0.677488 -0.596249 -0.640809 -0.637006  0.198606 -0.192079  0.213734  0.209820  0.211866  0.309015  0.166088  0.271313 -0.364926 -0.552030 -0.543183 -0.606003 -0.548884 -0.224728 -0.028967  0.249922  0.072631  0.259693 -0.645596 -0.641078 -0.645793 -0.681267 -0.591312 -0.631410  0.503526  0.413804  0.754274  0.738650  0.792549  0.713354  0.789909  0.712417  0.803999 -0.698873 -0.718903 -0.952879 -1.042743 -0.945723 -0.702982 -0.952047  1.122757
        -0.147557  0.251488 -0.134810 -0.076238 -0.613647  0.021040  0.097262  0.028591  0.136684  0.253935  0.156986 -0.000441  0.240808  0.261814  0.042360  0.281563  0.371415  0.120110  0.253472  0.248883  0.010347  0.094674  0.018464  0.233346 -0.128312  0.113023 -0.026593 -0.050040  0.216199 -0.108395 -0.180123 -0.362084 -0.062586 -0.051614  0.100443 -0.048397 -0.206370 -0.301052 -0.083185  0.215810  0.204454 -0.239281  0.103616  0.043089 -0.328135 -0.255512 -0.027166  0.129841  0.136857  0.197743  0.142667  0.173516  0.279004  0.105463  0.278233  0.034604 -0.041022  0.188550 -0.291168 -0.240944  0.164787  0.082102  0.292340 -0.137002
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.014393  0.014933  0.013584  0.026807  0.014761  0.020107  0.018968  0.019766 -0.012303 -0.001086  0.001782 -0.005707 -0.011821 -0.010167  0.000690  0.000908  0.000795  0.000455 -0.005855 -0.001058 -0.002364 -0.005895 -0.005946 -0.010520 -0.010373 -0.007164 -0.018502 -0.025688 -0.024341 -0.026093 -0.032688 -0.039959 -0.046790 -0.032377 -0.027286 -0.030018 -0.027216 -0.021546 -0.034300 -0.022526 -0.029976 -0.033000 -0.027196 -0.035698 -0.037293 -0.045833 -0.051718 -0.051462 -0.042357 -0.040190 -0.040248 -0.047481 -0.044324 -0.038027 -0.034340 -0.030601 -0.034244 -0.017977 -0.020459 -0.026322 -0.034625 -0.044695 -0.050962 -0.058982 -0.060085 -0.050997 -0.050306 -0.049436 -0.059563 -0.064068 -0.064712 -0.057815 -0.061229 -0.054701 -0.054970 -0.049744 -0.051942 -0.067671 -0.068906 -0.066152 -0.069242 -0.062911 -0.063980 -0.063976 -0.070367 -0.055585 -0.071438 -0.040280 -0.037285 -0.046590 -0.046780 -0.046406 -0.039379 -0.038751 -0.053430 -0.064218 -0.046924 -0.037396 -0.048873 -0.039273 -0.017957 -0.033151 -0.033707 -0.022021 -0.025985 -0.037377 -0.045894 -0.037328 -0.013481 -0.017391 -0.017079  0.027457  0.022578  0.024595  0.019028  0.019013  0.019011  0.019155  0.018817  0.020511  0.007878  0.027415  0.006601  0.006577 -0.016324 -0.018171  0.006959  0.030483
        -0.082252 -0.079759 -0.074512 -0.069811 -0.064785 -0.059966 -0.051606 -0.048799  0.105776  0.112714  0.118205  0.122577  0.124240  0.129015  0.134953  0.135612  0.135585  0.135605  0.135078 -0.077065 -0.071814 -0.067183 -0.064821 -0.050055 -0.043551 -0.033949 -0.022091 -0.033855 -0.045150 -0.055978 -0.058557 -0.045881 -0.040327  0.118124  0.125251  0.114127  0.113159  0.115896  0.106495  0.108688  0.095068  0.099974  0.089527  0.061778  0.072991  0.078077  0.067349  0.044473  0.052733  0.037298  0.018089  0.022856 -0.010692 -0.003838 -0.007563  0.011315  0.037436 -0.001783 -0.015471 -0.012957 -0.025266 -0.027877 -0.008116 -0.017043  0.005616  0.008170  0.020161  0.030973  0.033048  0.044625  0.061708  0.056420  0.071565  0.092412  0.094237  0.098674  0.100951  0.083716  0.073734  0.080424  0.034194  0.020285  0.001805 -0.010249 -0.001092 -0.025894  0.049684  0.114547  0.111129  0.102231  0.097016  0.095106  0.100941  0.098697  0.090391  0.082677  0.032873 -0.037064  0.040355  0.087103  0.103263  0.110229  0.113874  0.126973 -0.072028 -0.064618 -0.051677 -0.042009 -0.038382 -0.048555 -0.073999  0.141126  0.138114  0.137197  0.137202  0.138090  0.138705  0.136786  0.132029  0.127750  0.128169  0.141340  0.136256  0.137037  0.121071  0.098279 -0.070132 -0.080649
        -0.265167 -0.271630 -0.285033 -0.297702 -0.310167 -0.322953 -0.341672 -0.346755 -0.329091 -0.328404 -0.323628 -0.308502 -0.296317 -0.291376 -0.290210 -0.278267 -0.265594 -0.252875 -0.239977 -0.276873 -0.290595 -0.301683 -0.307459 -0.327414 -0.332835 -0.340331 -0.336798 -0.321805 -0.315539 -0.304320 -0.288002 -0.285590 -0.277674 -0.253119 -0.246637 -0.275888 -0.289800 -0.297377 -0.288415 -0.309394 -0.326668 -0.314855 -0.335295 -0.327971 -0.327841 -0.312958 -0.298419 -0.297800 -0.315176 -0.320932 -0.322988 -0.308124 -0.312809 -0.327498 -0.330778 -0.340845 -0.332692 -0.347654 -0.338836 -0.335179 -0.316622 -0.295844 -0.302592 -0.284964 -0.285113 -0.302822 -0.302836 -0.303338 -0.283049 -0.272820 -0.271278 -0.284753 -0.281644 -0.283932 -0.275667 -0.271908 -0.259487 -0.264314 -0.266384 -0.272485 -0.263753 -0.277860 -0.277832 -0.277919 -0.265508 -0.279436 -0.257759 -0.246831 -0.265388 -0.266369 -0.291557 -0.303050 -0.297137 -0.308800 -0.296364 -0.277577 -0.308096 -0.298695 -0.303393 -0.322046 -0.327367 -0.277541 -0.265092 -0.252690 -0.276575 -0.270969 -0.269567 -0.293709 -0.332946 -0.321987 -0.280769 -0.237108 -0.246114 -0.265641 -0.272275 -0.285230 -0.297941 -0.309679 -0.319190 -0.324364 -0.314360 -0.240255 -0.259327 -0.290943 -0.295965 -0.333690 -0.295774 -0.269480
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.013240 -0.011058 -0.014397  0.071996 -0.013339 -0.011798 -0.078621 -0.189575 -0.439156 -0.471251 -0.473627 -0.505983 -0.580084 -0.610661 -0.374573 -0.263493 -0.199579 -0.102421 -0.090028 -0.062711 -0.044263 -0.111957 -0.127223 -0.556374 -0.608706 -0.593209 -0.615059 -0.735035 -0.792561 -0.779848 -0.787619 -0.856514 -0.863079 -0.628757 -0.600754 -0.702827 -0.735472 -0.685655 -0.666595 -0.577967 -0.594667 -0.551604 -0.595769 -0.921994 -0.873651 -0.911460 -0.904381 -0.943521 -0.929644 -0.931689 -0.931581 -0.938966 -0.889990 -0.871576 -0.711612 -0.856445 -0.925489 -0.576385 -0.603813 -0.646816 -0.769903 -0.793351 -0.903669 -0.850911 -0.951451 -0.941586 -0.941788 -0.940904 -0.951059 -0.956123 -0.945244 -0.951401 -0.941329 -0.719319 -0.671698 -0.574629 -0.660595 -0.951257 -1.008576 -0.996204 -0.960725 -0.954395 -0.956058 -0.968441 -0.965151 -0.747394 -0.964805 -0.597048 -0.650927 -0.634307 -0.611969 -0.630411 -0.574336 -0.558220 -0.832721 -0.966058 -0.938394 -0.848725 -0.940347 -0.821939 -0.497556 -0.748899 -0.614365 -0.590682 -0.508564 -0.740622 -0.865526 -0.860133 -0.655589 -0.658413 -0.233924 -0.163505 -0.310192 -0.102489 -0.006849 -0.001435 -0.070907 -0.279721 -0.312676 -0.306419 -0.500176 -0.212982 -0.056048 -0.245086 -0.604679 -0.481482 -0.023627  0.100078
        -1.014332 -1.011848 -1.006294 -1.003646 -0.996570 -0.986437 -0.932871 -0.856959  0.846803  0.825386  0.834899  0.893986  0.864920  0.832222  1.049264  1.099983  1.115197  1.130274  1.131529 -1.012158 -1.004283 -0.985306 -0.983540 -0.624846 -0.496509 -0.364821 -0.409610 -0.462454 -0.417544 -0.536616 -0.558768 -0.405245 -0.442396  0.869509  0.892253  0.825357  0.778027  0.783762  0.859457  0.876256  0.725221  0.886670  0.680005  0.024179 -0.070669  0.020311 -0.124990  0.093581  0.061564  0.088168  0.068508  0.073738 -0.268693 -0.120839 -0.331305 -0.099879  0.088300 -0.370485 -0.397114 -0.397102 -0.501460 -0.538123 -0.174245 -0.374499  0.054069  0.053315  0.071045  0.081826  0.083917  0.093709 -0.025864  0.022275 -0.132471  0.816738  0.848594  0.932574  0.832760  0.459165  0.081640  0.331776  0.085080  0.071158  0.044344 -0.088207  0.041338 -0.625226  0.073140  0.899979  0.854904  0.856214  0.910722  0.883354  0.928986  0.911871  0.677249  0.413240  0.083712 -0.460228  0.091179  0.427966  0.812265  0.765329  0.903323  0.905761 -0.899529 -0.678743 -0.437059 -0.427373 -0.392167 -0.542217 -1.000648  1.121039  1.070845  1.128952  1.135814  1.136291  1.134442  1.028076  0.912162  0.767066  0.844092  1.111995  1.133931  1.102286  0.868668  0.792043 -1.001801 -0.992243
        -0.626362 -0.632926 -0.646970 -0.652539 -0.672089 -0.697964 -0.804109 -0.897257 -0.847430 -0.849023 -0.833859 -0.701760 -0.654744 -0.672043 -0.442575 -0.269210 -0.280125 -0.245741 -0.240551 -0.625874 -0.649409 -0.683521 -0.683287 -0.937053 -0.993763 -1.079976 -1.039614 -0.881385 -0.836269 -0.752473 -0.712110 -0.737358 -0.692418 -0.535523 -0.534458 -0.479554 -0.527126 -0.633403 -0.470763 -0.629275 -0.859614 -0.649757 -0.908061 -0.789561 -0.856866 -0.810306 -0.784189 -0.747043 -0.776310 -0.771088 -0.773525 -0.758309 -0.780019 -0.867412 -0.991454 -0.893673 -0.783353 -1.090782 -1.055806 -1.018861 -0.798914 -0.719121 -0.797861 -0.780013 -0.735801 -0.755385 -0.753024 -0.753558 -0.733212 -0.722074 -0.737105 -0.732344 -0.710356 -0.467328 -0.500597 -0.442516 -0.566076 -0.544159 -0.608374 -0.540489 -0.713941 -0.728047 -0.727705 -0.697282 -0.709987 -0.682178 -0.706475 -0.517212 -0.530418 -0.559790 -0.427366 -0.496965 -0.464955 -0.571290 -0.516146 -0.555809 -0.758315 -0.702027 -0.753604 -0.842859 -0.844379 -0.519509 -0.464105 -0.517479 -0.563556 -0.629156 -0.693458 -0.711425 -1.013044 -0.909685 -0.587853 -0.179628 -0.384955 -0.282109 -0.226402 -0.228868 -0.318474 -0.650669 -0.849761 -1.020347 -0.793249 -0.234055 -0.232505 -0.361370 -0.604085 -0.885082 -0.657791 -0.674638
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.999166 0.040392 0.005912]';
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.144373 -0.981374 -0.126733]';
q1a = [-0.023736 0.010731 -0.115211]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [0.230494 -0.157904 0.960177]';
q1b = [-0.023736 0.010731 -0.115211]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [-0.962304 0.109412 0.248997]';
q1c = [-0.023736 0.010731 -0.115211]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [-0.200617 -0.973258 -0.111897]';
q2a = [0.002415 -0.007184 -0.006271]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [0.029357 -0.120140 0.992323]';
q2b = [0.002415 -0.007184 -0.006271]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [-0.979230 0.195792 0.052674]';
q2c = [0.002415 -0.007184 -0.006271]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [-0.092138 -0.988849 -0.116993]';
q3a = [0.006308 -0.023118 0.125336]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [-0.554327 -0.046666 0.830989]';
q3b = [0.006308 -0.023118 0.125336]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [-0.827183 0.141418 -0.543847]';
q3c = [0.006308 -0.023118 0.125336]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [-0.034373 -0.996292 -0.078878]';
q4a = [-0.027136 -0.025934 0.175473]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [0.848419 -0.070802 0.524569]';
q4b = [-0.027136 -0.025934 0.175473]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [-0.528208 -0.048891 0.847706]';
q4c = [-0.027136 -0.025934 0.175473]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.144373 0.981374 0.126733]';
q5a = [-0.023736 0.010731 -0.115211]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [-0.030522 0.132430 -0.990722]';
q5b = [-0.023736 0.010731 -0.115211]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [-0.989053 0.139165 0.049072]';
q5c = [-0.023736 0.010731 -0.115211]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.049218 0.989783 0.133821]';
q6a = [-0.027365 0.026475 -0.232989]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [0.462962 0.141329 -0.875039]';
q6b = [-0.027365 0.026475 -0.232989]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [-0.885011 0.018886 -0.465188]';
q6c = [-0.027365 0.026475 -0.232989]';
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
