function [new_config, new_ctrm, new_norms]  = cicero_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.002764  0.005804  0.015107  0.014045  0.003342 -0.020860 -0.015258 -0.031529 -0.021339 -0.021279 -0.011115 -0.024749 -0.011054 -0.008858 -0.012011 -0.021266 -0.061188 -0.061232 -0.062642 -0.053378 -0.056225 -0.055818 -0.051005 -0.099711 -0.097519 -0.092144 -0.100377 -0.101718 -0.049440 -0.056757 -0.054789 -0.054230 -0.052858 -0.063597 -0.039975 -0.070464 -0.081725 -0.042681 -0.110632 -0.108019 -0.111866 -0.109683 -0.121889 -0.130721 -0.126189 -0.121613 -0.121821 -0.125540 -0.116597 -0.105864 -0.107405 -0.104789 -0.105232 -0.116378 -0.114587 -0.116663 -0.122665 -0.118069 -0.119872 -0.132921 -0.130417 -0.129252 -0.125930 -0.121064 -0.117036 -0.122091 -0.134777 -0.135613 -0.139185 -0.137235 -0.137122 -0.141383 -0.139900 -0.142463 -0.139113 -0.129270 -0.116035 -0.114388 -0.114548 -0.117739 -0.117549 -0.092531 -0.093164 -0.090002 -0.096412 -0.100238 -0.094880 -0.089825 -0.098487 -0.109643 -0.103673 -0.111544 -0.115279 -0.072952 -0.100997 -0.079708 -0.076656 -0.064775 -0.078767 -0.074308 -0.081030 -0.083113 -0.101971 -0.095413 -0.102228 -0.109342 -0.112362 -0.114498 -0.131509 -0.115710 -0.116109 -0.118445 -0.112249 -0.087836 -0.107234 -0.073700 -0.035334 -0.044402 -0.075236 -0.068047 -0.065957 -0.004433 -0.036528 -0.034969 -0.033209 -0.003332 -0.003994 -0.004832
        -0.119480 -0.124978 -0.131801 -0.135474 -0.137918 -0.132935 -0.128331 -0.123943 -0.120945 -0.116240 -0.118185  0.171468  0.170834  0.167630  0.163090  0.152901  0.139830  0.138740  0.144631  0.151765  0.154825  0.159666  0.162682 -0.094815 -0.097710 -0.104249 -0.101575 -0.102304 -0.127520 -0.120577 -0.114188 -0.109725 -0.108577 -0.101879 -0.110518 -0.111972 -0.112103 -0.129604 -0.083845 -0.083381 -0.074119 -0.095442 -0.080400 -0.060398 -0.066261  0.068149  0.061995  0.045486  0.072763  0.073020  0.055907  0.037152  0.027137 -0.029327 -0.042639 -0.026125 -0.012940  0.012079  0.027829  0.011262  0.002694 -0.025449 -0.031599 -0.046481 -0.058328 -0.055048 -0.048557 -0.036806 -0.024252  0.025183  0.013492 -0.007244 -0.013389  0.003664 -0.030818 -0.014486  0.034130 -0.050022 -0.046060 -0.021008 -0.002162  0.075139  0.064437  0.073673  0.091179  0.109152  0.129153  0.135831  0.126120  0.101961  0.112469  0.091652  0.079544  0.149290  0.119854  0.131712  0.125828  0.133157  0.100013  0.122677  0.086530  0.108996  0.099208  0.056236  0.051445  0.038232  0.049426  0.056199  0.043446 -0.083069 -0.080878 -0.073970 -0.095424 -0.112193 -0.097127 -0.098924  0.167741  0.159478  0.146686  0.139267  0.135413  0.152751  0.158930 -0.112160 -0.128493 -0.134896 -0.123880 -0.115904
        -0.092274 -0.047779 -0.016609  0.009175  0.047953  0.054653  0.012295  0.015075 -0.029713 -0.079048 -0.085842 -0.004616  0.040322  0.068821  0.088768  0.125406  0.133667  0.127837  0.078429  0.073062  0.037914 -0.006014 -0.014599 -0.062887 -0.036331 -0.005850  0.029380  0.064800  0.067054  0.040052 -0.010484 -0.043287 -0.052627 -0.085762 -0.084210 -0.000525  0.024086  0.067170  0.092449  0.044272  0.038541  0.006653 -0.017253 -0.020917 -0.066073 -0.017403 -0.006937  0.029952  0.027715  0.056086  0.062425  0.079643  0.098136  0.098065  0.069950  0.063265  0.053503  0.064324  0.052888  0.025712  0.030252  0.014783  0.021943  0.035380  0.045925  0.016783 -0.023983 -0.046256 -0.042447 -0.024160  0.008307 -0.023340 -0.015073 -0.042980 -0.033102  0.023076  0.057109  0.061030  0.082742  0.096984  0.097957  0.133914  0.101082  0.091957  0.065001  0.045771  0.030360  0.020714 -0.009569 -0.015217  0.012272  0.023603  0.011309  0.013837  0.028511  0.070225  0.081469  0.100968  0.123522  0.136633  0.115714  0.081791  0.049325  0.099858  0.115865  0.127362  0.056074  0.047946 -0.006322 -0.077503 -0.052498 -0.041099 -0.004623  0.049684 -0.016528 -0.085152  0.002979  0.044891  0.027886  0.080292  0.135338  0.125951  0.081358 -0.072875  0.048133  0.038195 -0.039895 -0.119632
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.126749 -0.183170 -0.260639 -0.239892 -0.229024 -0.263892 -0.280792 -0.295649 -0.267227 -0.266107 -0.170043 -0.239017 -0.107312  0.086314 -0.022920 -0.005595 -0.707944 -0.816628 -0.689332 -0.542048 -0.476174 -0.465376 -0.453577 -0.480014 -0.541979 -0.410722 -0.790363 -0.932644 -0.236135 -0.297290 -0.319979 -0.331781 -0.328168 -0.367053 -0.390692 -0.335365 -0.416320 -0.327536 -1.098385 -1.048473 -1.053645 -0.756523 -0.989684 -1.059162 -1.043273 -0.949073 -0.913807 -1.030065 -1.034694 -0.975758 -0.976731 -0.984244 -0.931045 -1.099921 -1.114042 -1.073761 -1.067101 -1.005506 -0.991665 -1.075469 -1.080208 -1.066970 -1.080662 -1.091044 -1.062638 -1.060118 -1.091404 -1.085665 -1.112513 -1.052314 -1.132761 -1.134428 -1.123312 -1.141278 -1.110922 -1.081317 -0.988370 -1.094862 -1.111946 -1.098031 -1.098736 -0.948934 -1.018297 -1.001157 -0.965348 -0.965850 -0.907293 -0.829163 -0.940423 -1.023139 -1.035919 -1.076536 -1.032007 -0.562005 -0.971175 -0.732727 -0.862868 -0.808219 -0.973769 -0.908420 -0.965081 -0.955995 -0.969151 -1.053517 -1.005951 -1.065178 -0.981732 -0.988641 -1.069073 -0.785194 -0.944587 -0.983538 -0.712178 -0.553276 -0.591429 -0.329850 -0.397039 -0.453972 -0.579839 -0.798574 -0.824258 -0.047127 -0.407537 -0.346004 -0.326939 -0.247843 -0.219017 -0.014796
        -1.096893 -1.101205 -1.081477 -1.095247 -1.104304 -1.092941 -1.083570 -1.081462 -1.083915 -1.085768 -1.104677  1.147995  1.164446  1.127744  1.145073  1.094161  0.901321  0.765909  0.901169  1.014866  1.056625  1.066917  1.074902 -1.017489 -0.985782 -1.047419 -0.825103 -0.604141 -1.037855 -1.080880 -1.064251 -1.058717 -1.057433 -1.035409 -1.046920 -1.058314 -1.049584 -1.018155 -0.238448 -0.409343 -0.372427 -0.838848 -0.539984 -0.431863 -0.405599  0.515779  0.641347  0.324877  0.421931  0.312411  0.294904  0.362282  0.589700 -0.189797 -0.070926 -0.189158 -0.129899  0.186243  0.261079 -0.048716 -0.114817 -0.228133 -0.231156 -0.280298 -0.357088 -0.263193 -0.337964 -0.312733 -0.215153  0.358016 -0.030840 -0.028495 -0.105410 -0.039384 -0.242248 -0.202546  0.272383 -0.154227 -0.020135 -0.146202  0.185120  0.588165  0.437551  0.428330  0.332287  0.438445  0.700531  0.808221  0.590944  0.390774  0.463073  0.353758  0.478930  1.017204  0.599164  0.725809  0.585173  0.663954  0.436728  0.528657  0.528279  0.350763  0.365042  0.283968  0.375505  0.290769  0.288875  0.306480  0.391188 -0.813800 -0.610295 -0.563892 -0.893341 -0.992300 -0.945089 -1.040637  1.097657  1.066172  1.002713  0.782322  0.770015  1.114079  1.076363 -1.062313 -1.072977 -1.092371 -1.093923 -1.095503
        -0.259276 -0.153935 -0.165201 -0.110628 -0.062071 -0.084356 -0.118121 -0.100678 -0.140300 -0.087742 -0.125536  0.017425  0.099222  0.331731  0.277424  0.462726  0.090785 -0.061996  0.265271  0.200575  0.139891  0.089685  0.061494 -0.126466 -0.153724 -0.100399  0.050012 -0.175452 -0.302318 -0.101235 -0.174968 -0.192889 -0.207149 -0.276644 -0.072068 -0.185633 -0.071697 -0.292462  0.071428  0.140680  0.193666  0.176788 -0.206237 -0.024228  0.143229  0.321619  0.185699  0.352092  0.215276  0.487335  0.495036  0.427277  0.137585  0.015038  0.086927  0.302814  0.360684  0.491082  0.483667  0.354353  0.320231  0.296923  0.242544  0.102223  0.174687  0.293875  0.009310 -0.192081 -0.169686  0.203552  0.090388  0.092465  0.141237 -0.020264 -0.137428  0.264402  0.484037  0.227797  0.150009 -0.055839  0.051081  0.075793  0.030986  0.301756  0.497225  0.422973  0.146600  0.056355  0.264442  0.271334 -0.077138  0.014018  0.021316  0.100738  0.142760  0.539921  0.494837  0.507842 -0.169044 -0.236781 -0.036965  0.505614  0.470431 -0.073853 -0.163902 -0.022980  0.488347  0.464158 -0.013242 -0.211003  0.130101 -0.148763  0.053802 -0.044009 -0.232203 -0.303245  0.069509  0.145671  0.140113  0.310099 -0.013854  0.398026  0.225123 -0.094472 -0.099081 -0.114955 -0.152951 -0.320346
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.024261 -0.064449 -0.056831 -0.052996 -0.062128 -0.096263 -0.104745 -0.085263 -0.061877 -0.045494 -0.049201 -0.051396 -0.039144 -0.073824 -0.126176 -0.116359 -0.127022 -0.124255 -0.114117 -0.123200 -0.120745 -0.122995 -0.137949 -0.141046 -0.145220 -0.149002 -0.152496 -0.158186 -0.154902 -0.152805 -0.142768 -0.138628 -0.143912 -0.149899 -0.141882 -0.135423 -0.137477 -0.135362 -0.147262 -0.135694 -0.153239 -0.151770 -0.129836 -0.143406 -0.146519 -0.152702 -0.142642 -0.136890 -0.133366 -0.091321 -0.099900 -0.084426 -0.070712 -0.086250 -0.104763 -0.108284 -0.117500 -0.126461 -0.139062 -0.086727 -0.074726 -0.074885 -0.071851 -0.028750
        -0.093385  0.156814  0.158514  0.164646  0.174257 -0.061775 -0.072768 -0.086703 -0.088762 -0.084559 -0.075056 -0.069624 -0.073810 -0.091441 -0.059518 -0.074174 -0.049487  0.144278  0.141712  0.118696  0.102040  0.090273 -0.028852 -0.029983 -0.031491 -0.023387  0.003646  0.029047  0.034345  0.056915  0.049193  0.051426  0.010561  0.025647 -0.002386  0.061272  0.089255  0.100372  0.108666  0.116698  0.072550  0.065166  0.108656  0.073453 -0.008142 -0.004046 -0.013621 -0.022576 -0.048236  0.150430  0.152167  0.160371  0.168456  0.156103  0.140389  0.126604  0.101882  0.125900 -0.042788 -0.092219 -0.068272  0.163355  0.155660 -0.093205
        -0.153054 -0.091829 -0.101160 -0.119157 -0.172042 -0.194461 -0.158783 -0.142260 -0.135554 -0.155176 -0.199023 -0.211992 -0.198542 -0.128910 -0.128369 -0.139242 -0.182455 -0.147400 -0.120960 -0.104471 -0.087257 -0.077875 -0.104002 -0.149705 -0.163805 -0.167827 -0.155180 -0.147717 -0.134000 -0.128741 -0.103641 -0.076307 -0.110409 -0.123827 -0.104799 -0.090706 -0.115423 -0.117456 -0.146351 -0.127523 -0.137455 -0.130507 -0.111251 -0.118356 -0.144310 -0.163744 -0.130300 -0.112225 -0.103774 -0.091205 -0.128068 -0.136963 -0.154079 -0.110855 -0.094443 -0.065892 -0.079412 -0.118131 -0.178494 -0.125940 -0.199088 -0.129588 -0.083517 -0.146755
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.564971 -0.129396 -0.056233 -0.157192 -0.473624 -0.507977 -0.521372 -0.324273  0.002665 -0.162205 -0.098800 -0.222838  0.124575  0.071918 -1.035568 -0.732502 -0.601144 -0.371625 -0.763693 -0.982558 -0.982574 -1.019544 -1.117958 -1.062465 -1.053703 -1.064463 -1.084660 -1.098262 -1.093915 -1.039885 -1.076608 -1.075869 -1.126642 -1.085175 -1.130915 -1.056399 -1.003659 -1.001862 -1.018364 -0.967973 -1.024609 -1.023496 -0.990546 -1.011847 -1.085688 -1.070830 -1.095200 -1.131339 -1.056094 -0.561798 -0.542152 -0.495790 -0.478072 -0.459453 -0.836230 -1.011201 -1.035866 -0.934114 -0.811532 -0.354646 -0.369768 -0.399649 -0.253463 -0.547471
        -0.839340  1.148960  1.125142  1.100485  1.080675 -0.897930 -0.912529 -0.996125 -1.071466 -1.042048 -1.026490 -0.987182 -1.041784 -1.076388 -0.462696 -0.822466 -0.888912  1.005939  0.876366  0.444626  0.454878  0.500166 -0.113178 -0.291668 -0.365730 -0.301161 -0.283855 -0.187040 -0.104350  0.228824  0.229240  0.399769 -0.140909 -0.152595 -0.143512  0.411993  0.334164  0.344775  0.385608  0.422201  0.308368  0.288833  0.382042  0.314302 -0.308994 -0.253671 -0.272657 -0.099587 -0.417368  1.019476  1.038688  1.065791  1.076178  1.072623  0.787022  0.556291  0.429843  0.586671 -0.779191 -1.046849 -0.949636  1.092446  1.139030 -0.852386
        -0.541878  0.015079  0.155023  0.217517 -0.076773 -0.556865 -0.506953 -0.482593 -0.309125 -0.418980 -0.502855 -0.570733 -0.388867 -0.221859 -0.026124 -0.385060 -0.448112  0.295711  0.074834  0.289577  0.277098  0.090017 -0.284197  0.137537  0.087078  0.123328  0.064850  0.116029  0.180672  0.299671  0.205434 -0.091710 -0.004114  0.181932 -0.148343  0.078992  0.320179  0.317797  0.259216  0.335058  0.292781  0.305458  0.318212  0.315005  0.021374  0.144036  0.029493 -0.040527 -0.214746  0.061803  0.007923 -0.032118 -0.053591  0.033053  0.121943 -0.076372  0.142049  0.249819 -0.252608 -0.255944 -0.568068  0.047381 -0.083004 -0.539913
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.031861 -0.017365 -0.011112 -0.034464 -0.025867 -0.015701 -0.015125 -0.013944 -0.029499 -0.016077 -0.016420 -0.066820 -0.057645 -0.046925 -0.063075 -0.058961 -0.055476 -0.057418 -0.044672 -0.049244 -0.055443 -0.062336 -0.051986 -0.047395 -0.097587 -0.093357 -0.090758 -0.096827 -0.051427 -0.127108 -0.119865 -0.114822 -0.131042 -0.150904 -0.133542 -0.153928 -0.152551 -0.143683 -0.119766 -0.131500 -0.138044 -0.139555 -0.137019 -0.124768 -0.138724 -0.149363 -0.174534 -0.163387 -0.173579 -0.186050 -0.188687 -0.182839 -0.179793 -0.179597 -0.182502 -0.165318 -0.189500 -0.173181 -0.170668 -0.180259 -0.204851 -0.204181 -0.195622 -0.195582 -0.197809 -0.205427 -0.204761 -0.202484 -0.222546 -0.212849 -0.214437 -0.222204 -0.206002 -0.202941 -0.204330 -0.225494 -0.206082 -0.222926 -0.213401 -0.215259 -0.204369 -0.196924 -0.198542 -0.220697 -0.213723 -0.219156 -0.208310 -0.215247 -0.235358 -0.232009 -0.238716 -0.230907 -0.233619 -0.238739 -0.252346 -0.245533 -0.237547 -0.251437 -0.240538 -0.250748 -0.266085 -0.281031 -0.290116 -0.291013 -0.309631 -0.295306 -0.309150 -0.294883 -0.313341 -0.319827 -0.309893 -0.301167 -0.309156 -0.289951 -0.294762 -0.303361 -0.299224 -0.332774 -0.331461 -0.348965 -0.318842 -0.323479 -0.336759 -0.319172 -0.319707 -0.323019 -0.283580 -0.278330 -0.270302 -0.278154 -0.273488 -0.255746 -0.268682 -0.260699 -0.266439 -0.260545 -0.266132 -0.257143 -0.257991 -0.266834 -0.265970 -0.258832 -0.256124 -0.256137 -0.270135 -0.259730 -0.254911 -0.237895 -0.221023 -0.234432 -0.242868 -0.249774 -0.241277 -0.226582 -0.248057 -0.238616 -0.250514 -0.241148 -0.240014 -0.233158 -0.281129 -0.239829 -0.237113 -0.292556 -0.284911 -0.248905 -0.319045 -0.285014 -0.341087 -0.279218 -0.314579 -0.283478 -0.237230 -0.230492 -0.234514 -0.264000 -0.212390 -0.214200 -0.206642 -0.210149 -0.247758 -0.247152 -0.229196 -0.225135 -0.189638 -0.219745 -0.185514 -0.192668 -0.215903 -0.191147 -0.225583 -0.201081 -0.210972 -0.185563 -0.208514 -0.162172 -0.151682 -0.157948 -0.158412 -0.167467 -0.143126 -0.169665 -0.163873 -0.185953 -0.161797 -0.195147 -0.143182 -0.095778 -0.096296 -0.103021 -0.080309 -0.097142 -0.100985 -0.090743 -0.090613 -0.083451 -0.104329 -0.119569 -0.116869 -0.118672 -0.101218 -0.072894 -0.071894 -0.107751 -0.076920 -0.120554 -0.108292 -0.140910 -0.143504 -0.110056 -0.145658 -0.136019 -0.142201 -0.110574 -0.063361 -0.038706 -0.041145 -0.032626 -0.074809 -0.073808 -0.002295 -0.008147 -0.011955 -0.001377  0.003849  0.008647  0.006539  0.003388 -0.011250 -0.027791 -0.009502 -0.011866 -0.007181 -0.039708 -0.035384 -0.037275
         0.022735  0.036732  0.069478  0.091814  0.117661  0.131889  0.155018  0.159976  0.179281  0.171158  0.171612  0.196392  0.189122  0.199622  0.190413  0.188489  0.165921  0.155450  0.139060  0.116859  0.089333  0.061786  0.051364  0.028725  0.047026  0.020770  0.003028 -0.005881  0.007309 -0.011976 -0.009231  0.011071  0.041291  0.047142  0.066031  0.094382  0.121799  0.125749  0.142005  0.167317  0.180761  0.198728  0.195939  0.194549  0.183351 -0.005833  0.008925  0.023954  0.029990  0.045576  0.075074  0.090339  0.108507  0.127129  0.149938  0.156251  0.166338  0.182897  0.179280  0.172125  0.101509  0.022775 -0.003567 -0.004427 -0.003074  0.004128  0.018588  0.023555  0.038181  0.048566  0.081167  0.098065  0.113192  0.126673  0.149512  0.177822  0.180362  0.174956  0.160781  0.137380  0.154232  0.162619  0.157964  0.116318  0.108302  0.071968  0.060184  0.039978  0.033950  0.008751  0.004392 -0.003749 -0.007062 -0.002249  0.005173  0.051607  0.064122  0.090547  0.097378  0.128721  0.121972  0.118024  0.169557  0.180065  0.168528  0.134773  0.108239  0.086123  0.064576  0.044173  0.040286  0.029449  0.017745  0.014373  0.006724  0.006662  0.027810  0.107518  0.071973  0.053096 -0.000453  0.012906  0.018069  0.021019  0.086796  0.125907  0.102059  0.058339  0.050713  0.007550  0.003595  0.004634  0.016525  0.023792  0.033233  0.046446  0.086932  0.090781  0.108870  0.152359  0.160973  0.170950  0.184972  0.185795  0.176358  0.153236  0.153089  0.152239  0.133112  0.159669  0.176598  0.183037  0.182053  0.187757  0.176809  0.167349  0.142664  0.125395  0.097230  0.143391  0.142171  0.081279  0.064686  0.055947  0.031984  0.009557  0.003903  0.018001  0.055084  0.129548  0.156866  0.165188  0.106731  0.063318  0.046772  0.032787 -0.003242 -0.008553 -0.006629  0.001023 -0.003617  0.013915  0.049876  0.129895  0.167799  0.186952  0.167701  0.158444  0.160700  0.123650  0.072858  0.039319  0.002444 -0.003246  0.086357  0.179842  0.186348  0.184842  0.189319  0.175281  0.150741  0.186124  0.143034  0.123985  0.077212  0.033355 -0.002454  0.203895  0.205410  0.207852  0.201597  0.182356  0.162012  0.136709  0.131109  0.110106  0.093068  0.071974  0.086313  0.112122  0.110423  0.143532  0.168486  0.169005  0.191453  0.204941  0.208860  0.185601  0.194290  0.147433  0.110597  0.019366 -0.009417  0.009457  0.054028  0.134459  0.185406  0.191600  0.200689  0.200206  0.137847  0.153610  0.162081  0.119556  0.105096  0.091872  0.121095  0.080514  0.164552  0.189757  0.151551  0.148384  0.127236  0.100678  0.071773  0.012080
        -0.408101 -0.412145 -0.427783 -0.457541 -0.479199 -0.476705 -0.465647 -0.452085 -0.434781 -0.412512 -0.390500 -0.354732 -0.365698 -0.401297 -0.423325 -0.427384 -0.457697 -0.464850 -0.476927 -0.478046 -0.451396 -0.428123 -0.422702 -0.415504 -0.420774 -0.405443 -0.381062 -0.349224 -0.399524 -0.340797 -0.349190 -0.391758 -0.404578 -0.391401 -0.417666 -0.401645 -0.389727 -0.398800 -0.413949 -0.398665 -0.383695 -0.364092 -0.329563 -0.304139 -0.268941 -0.342333 -0.372003 -0.382061 -0.387726 -0.399190 -0.409400 -0.412738 -0.409482 -0.400095 -0.388587 -0.371831 -0.348984 -0.329291 -0.304304 -0.277177 -0.269024 -0.297043 -0.325431 -0.331275 -0.365331 -0.402955 -0.405888 -0.403941 -0.436203 -0.417501 -0.424932 -0.433822 -0.425266 -0.420645 -0.408254 -0.406213 -0.382277 -0.346394 -0.321661 -0.293655 -0.278963 -0.260420 -0.295991 -0.296100 -0.287727 -0.289239 -0.283330 -0.294821 -0.305867 -0.337097 -0.356709 -0.365842 -0.395730 -0.417395 -0.425010 -0.456870 -0.454187 -0.467129 -0.450391 -0.452923 -0.460434 -0.463178 -0.436214 -0.393603 -0.372720 -0.338597 -0.332038 -0.318660 -0.323224 -0.324557 -0.322435 -0.332575 -0.346841 -0.361031 -0.391690 -0.426904 -0.452925 -0.341579 -0.325981 -0.324879 -0.414732 -0.369896 -0.355919 -0.340654 -0.330973 -0.344294 -0.472277 -0.474594 -0.467568 -0.426769 -0.400551 -0.383950 -0.359481 -0.338526 -0.319231 -0.303491 -0.314531 -0.313958 -0.319987 -0.337713 -0.344824 -0.355642 -0.403498 -0.418006 -0.431035 -0.444570 -0.444084 -0.436514 -0.434937 -0.430510 -0.423646 -0.419886 -0.392579 -0.388187 -0.356141 -0.330205 -0.321527 -0.311978 -0.303960 -0.311067 -0.340744 -0.303856 -0.299148 -0.313270 -0.322165 -0.360813 -0.398839 -0.354579 -0.324550 -0.334879 -0.356112 -0.438380 -0.446215 -0.443834 -0.449160 -0.453053 -0.400609 -0.366585 -0.343170 -0.318690 -0.401569 -0.348674 -0.297003 -0.301088 -0.313869 -0.376904 -0.376997 -0.394539 -0.415003 -0.414066 -0.431067 -0.406697 -0.410249 -0.305209 -0.283242 -0.286872 -0.303374 -0.312288 -0.353240 -0.355472 -0.391224 -0.337085 -0.379647 -0.409989 -0.402715 -0.401784 -0.352762 -0.343404 -0.349200 -0.378229 -0.387572 -0.406667 -0.416221 -0.434446 -0.437145 -0.455191 -0.435818 -0.427978 -0.430786 -0.422554 -0.436230 -0.451215 -0.435290 -0.409578 -0.411495 -0.388188 -0.371527 -0.276885 -0.338345 -0.416584 -0.399165 -0.378923 -0.341884 -0.391708 -0.426646 -0.479995 -0.431581 -0.404657 -0.383826 -0.357058 -0.402260 -0.415913 -0.424744 -0.455141 -0.441875 -0.431289 -0.411115 -0.440881 -0.402931 -0.410485 -0.447664 -0.473194 -0.468385 -0.467705 -0.430836 -0.399578
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.111407  0.205215  0.055730 -0.092924  0.036737  0.543562  0.820838  0.830475  0.401486  0.862165  0.507175  0.421984  0.624620  0.316922 -0.421505 -0.306594 -0.685650 -0.583135 -0.370732 -0.284291 -0.119059  0.172770  0.227404  0.173603 -0.283406 -0.259153 -0.109570 -0.124596  0.175971 -0.067886  0.009596 -0.458742 -0.618785 -0.487463 -0.762845 -0.394685 -0.635588 -0.946904 -0.591117 -0.804487 -0.858041 -0.590333 -0.526400 -0.174985 -0.185665 -0.539131  0.026742  0.020742  0.169079  0.256879  0.117709  0.197392  0.532534  0.542897  0.540667 -0.291754 -0.148923 -0.689761 -0.541134 -0.458819 -1.119327 -0.810217  0.059889  0.014172  0.232232  0.607413  0.621588  0.438351  0.591064  0.472839  0.334397  0.339756  0.347792  0.372584  0.427324  0.176566  0.296013 -0.052069 -0.178568 -0.900251 -1.080599 -0.622458 -0.968677 -0.925233 -1.057804 -0.722005 -0.772371 -0.652751 -0.521855 -0.860736 -0.809228 -0.681051 -0.348581 -0.425180 -0.291361  0.200675  0.459420  0.266970  0.606258  0.135754 -0.222538 -0.042861 -0.201961 -0.592769 -0.658122 -0.278145 -0.790285 -0.699695 -0.530303 -0.567174 -0.661847 -0.207387 -0.240915 -0.097693 -0.272084 -0.149536 -0.235171 -0.364654 -0.258282 -0.357728  0.112547 -0.299104 -0.438333 -0.370733 -0.531942 -0.797642 -0.085144 -0.277231  0.039844 -0.310427 -0.381560 -0.583292 -0.374879 -0.467864 -0.273171 -0.445445 -0.391389 -0.576244 -0.727757 -0.912439 -0.887528 -0.765080 -0.084689 -0.053822 -0.276782 -0.189673  0.004467  0.197309  0.324250  0.233993  0.185115  0.099520 -0.352779 -0.435554 -0.629812 -0.337914 -0.778048 -0.742384 -0.870884 -0.777601 -0.522573 -0.767139 -0.624880 -0.700997 -0.461805 -0.702518 -0.256479 -0.150272 -0.348979 -0.477992 -0.743268 -0.227209  0.405675  0.656618  0.358510 -0.120636  0.234998 -0.184442 -0.103011 -0.543081 -0.631851 -0.694935 -0.554999 -0.934207 -0.899353  0.018073  0.484421  0.410111  0.448626  0.341938  0.532534  0.320712  0.503922 -0.720581 -0.930769 -0.526021 -0.538959 -0.514579 -0.786371 -0.786184 -0.835024 -0.730832 -0.188209  0.466482 -0.167411  0.307892 -0.526989  0.077608  0.077026 -0.043778  0.067726 -0.390712 -0.495533 -0.677144 -0.699626 -0.848212 -0.552087 -0.525478 -0.603869 -0.641513 -0.755328 -0.878030 -0.691440 -0.250540 -0.598803 -0.572664 -0.159536 -0.287104 -0.490010 -0.557364 -0.961794 -0.535767 -0.450172 -0.277064  0.139968 -0.258169  0.142303  0.556224  0.159192  0.339609  0.930431  0.895266  0.841023  0.959669  0.967069  0.950163  0.917823  0.553823  0.912671  0.647626  0.908565  0.895657  0.804569 -0.049624 -0.030648  0.099179
        -0.538215 -0.204842 -0.691591 -0.682953 -0.235046  0.235623  0.666038  0.593649  1.025450  0.603326  0.849300  1.068179  0.915128  1.011934  0.876259  0.901642  0.652219  0.630034  0.445539 -0.148832 -0.731325 -0.233219 -0.146317 -0.329664 -0.445266 -0.587195 -0.935003 -0.952057 -0.894953 -0.988137 -0.973903 -0.686127 -0.627996 -0.417314 -0.148486  0.521846  0.275048  0.291137  0.477973  0.424260  0.477755  1.088292  1.110147  1.156346  1.133651 -0.819764 -0.832225 -0.420524 -0.353838 -0.184506 -0.252239 -0.109905  0.331535  0.495178  0.526979  0.692635  1.163300  1.031764  1.105328  1.110197 -0.202062 -0.343234 -0.926401 -0.982150 -0.898883 -0.422949 -0.005414 -0.117551  0.102247 -0.023136 -0.105782 -0.054498  0.102088  0.418207  0.573007  1.032813  1.040625  1.079488  1.081537  0.546156  0.589769  1.022998  0.794166  0.144396  0.009155  0.087354 -0.031072 -0.357427 -0.590120 -0.657812 -0.753520 -0.878729 -0.987626 -0.796800 -0.852419 -0.218958  0.022645  0.191011  0.239090  0.405700  0.599177  0.572941  0.772229  1.121262  1.039805  0.581878  0.316038  0.284458  0.305088 -0.290095 -0.522827 -0.727906 -0.835537 -0.897318 -0.952661 -0.910223 -0.636358  0.491588  0.334087 -0.153973 -0.892438 -0.911684 -0.942092 -0.796145  0.419112  0.379780  0.496842 -0.446811 -0.459099 -0.887922 -0.972635 -0.885438 -0.903546 -0.893923 -0.796623 -0.307289  0.493673  0.399740  0.341657  0.564832  0.794972  1.010071  1.134103  1.113651  0.902609  0.622012  0.672658  0.577728  0.362711  0.576094  0.873130  1.021381  1.175802  1.158474  1.083179  0.949033  0.358665  0.327401  0.136078  0.436618  0.558259  0.207271  0.171880  0.041142 -0.837540 -0.813479 -0.966694 -0.868230 -0.178738  0.534408  0.847759  0.667620  0.065308  0.224224 -0.113303 -0.559709 -0.838387 -1.006865 -0.999262 -0.632882 -0.922820 -0.789103 -0.165141  0.204002  0.871420  1.151482  0.854661  0.567772  0.533502  0.439295  0.010242 -0.164949 -0.560373 -0.846647  0.058607  1.102149  1.101041  1.111798  0.863223  0.754340  0.377588  1.012715  0.588241  0.424214 -0.051051 -0.178456 -0.746540  1.163938  1.169214  1.186509  1.186808  0.521230  0.583190  0.443349  0.526208  0.273569  0.117849 -0.169379  0.170481  0.298621  0.303292  0.420906  0.704443  0.753997  0.676853  1.039860  1.206947  1.139529  1.121375  0.499806  0.237208 -0.616990 -0.889374 -0.758601 -0.165580  0.330956  0.986179  0.939472  1.172755  1.107024  0.498429  0.578400  0.678399  0.318526  0.295831  0.148385  0.531510 -0.623547  0.520869  0.926592  0.414478  0.533140  0.120120 -0.594216 -0.644382 -0.741016
        -1.223460 -1.356656 -1.073001 -1.087079 -1.412836 -1.299179 -0.665707 -0.766542 -0.748229 -0.617259  0.125822 -0.387257 -0.279501  0.054515 -1.056684 -1.083197 -1.063002 -1.170819 -1.371218 -1.413012 -1.019263 -1.354242 -1.362310 -1.322542 -1.271139 -1.181903 -0.727105 -0.671683 -0.765876 -0.549608 -0.578642 -1.020751 -0.965080 -1.210552 -1.164633 -1.273029 -1.251811 -0.971061 -1.229396 -1.092257 -1.010911 -0.438078 -0.217288 -0.035020  0.038835 -0.773142 -0.873955 -1.258723 -1.245200 -1.265721 -1.303261 -1.315693 -1.074951 -0.985366 -0.967263 -1.206280 -0.282499 -0.217173 -0.232250 -0.071205 -0.001484  0.409184 -0.037176 -0.323483 -0.477541 -0.799055 -0.968534 -1.158540 -1.014073 -1.141856 -1.239690 -1.246796 -1.257845 -1.184699 -1.057611 -0.733837 -0.470862  0.044284  0.066920  0.309411 -0.485189  0.020059 -0.342224  0.413013  0.239242  0.574998  0.537345  0.511814  0.421083  0.063398 -0.040346 -0.187566 -0.236741 -0.995261 -0.937857 -1.309919 -1.170089 -1.316342 -0.963083 -1.332638 -1.338147 -1.321272 -1.229319 -0.241649 -0.027131  0.555720  0.519624  0.573972  0.622863  0.584885  0.425249  0.313659  0.170123  0.002089 -0.110504 -0.795226 -1.197759  0.581174  0.636277  0.653408 -0.279577  0.010287 -0.095588  0.233441  0.588006  0.498488 -1.369366 -1.337624 -1.270002 -0.870715 -0.212680 -0.066955  0.017602  0.000418  0.238707  0.613396  0.590385  0.581988  0.531559  0.304985  0.115296 -0.156673 -0.139378 -0.731299 -1.118432 -1.325105 -1.258192 -1.229960 -1.241139 -1.209719 -0.999557 -0.838422 -0.398178 -0.506679 -0.175152  0.285513  0.500088  0.529423  0.470955  0.474808  0.535940  0.536424  0.616355  0.599395  0.138960 -0.018986 -0.166401  0.088637  0.647698  0.557633  0.226033 -1.301164 -1.211040 -0.876435 -1.238272 -1.245766 -0.720572 -0.416452 -0.280393  0.379393 -0.314746  0.044583  0.623660  0.400143 -0.278913 -0.262342 -0.658551 -1.079454 -1.062628 -1.199043 -1.080173 -1.234952 -0.825176 -0.354508  0.407828 -0.156626 -0.187811 -0.195809 -0.742500 -0.886395 -1.076656 -0.379977 -1.274744 -1.105828 -1.394440 -1.239691 -0.899596 -0.123737 -0.146618 -0.575003 -0.473858 -1.300524 -1.232886 -1.184184 -1.124896 -1.078425 -1.329630 -1.309446 -1.300123 -1.254332 -1.167627 -0.975448 -1.008799 -1.207948 -1.112941 -0.702047 -0.406122 -0.014875 -0.195353 -1.238623 -0.962983 -1.038660 -0.703586 -1.010067 -1.380811 -1.435623 -1.001764 -0.098158 -0.402053 -0.274797 -0.400857 -0.357620 -0.348294 -0.646975 -0.631156 -0.763486 -0.377656 -0.889566 -0.542243 -0.380709 -0.744350 -0.641610 -1.052347 -1.186749 -1.128761 -1.043182
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.054739  0.063458  0.025719  0.024243  0.012325 -0.022349 -0.013564 -0.013939 -0.015681 -0.013024 -0.027603 -0.057218 -0.081754 -0.064578 -0.080199 -0.083258 -0.071074 -0.067402 -0.049498 -0.048276 -0.060703 -0.041781 -0.095619 -0.088203 -0.083350 -0.064879 -0.050329 -0.054760 -0.055884 -0.087649 -0.086521 -0.044319 -0.072427 -0.079404 -0.093130 -0.096907 -0.103958 -0.103558 -0.095709 -0.097990 -0.087343 -0.085217 -0.109555 -0.102454 -0.092582 -0.100914 -0.049190 -0.058085 -0.051413 -0.082087 -0.074574  0.040300  0.035208  0.025141  0.017709  0.000590 -0.032369 -0.008713 -0.038903 -0.003057  0.034518  0.041670  0.047867  0.048184
        -0.222255 -0.184746 -0.160333 -0.185071 -0.218307 -0.187978 -0.209748 -0.165556  0.111828  0.090531  0.056219 -0.068005 -0.066898 -0.026041  0.003610  0.021815  0.045157  0.031488  0.054287  0.074950  0.067778  0.121695 -0.138048 -0.135612 -0.119098 -0.119482 -0.177508 -0.194893 -0.155267 -0.160132 -0.161582 -0.199761 -0.159961 -0.139027 -0.095094 -0.103415 -0.092304 -0.059085 -0.049103 -0.024142  0.001684  0.001547 -0.065923 -0.092824 -0.102088 -0.129990  0.093859  0.004251  0.055652  0.011192 -0.101632  0.105745  0.117131  0.115647  0.097010  0.113427  0.052109  0.114920 -0.201001 -0.204092 -0.209901 -0.177426 -0.157249 -0.225333
         0.221037  0.150965  0.115294  0.173638  0.212671  0.266246  0.214747  0.131574  0.230438  0.291651  0.330830  0.309689  0.281336  0.303360  0.269136  0.204740  0.221313  0.279906  0.303984  0.294242  0.231504  0.211057  0.169841  0.229871  0.267913  0.298939  0.252531  0.192595  0.126622  0.174579  0.209445  0.189417  0.255030  0.269344  0.241054  0.224164  0.164896  0.220145  0.256850  0.244322  0.218655  0.254402  0.167891  0.171132  0.238276  0.187284  0.256486  0.306957  0.299397  0.238072  0.290223  0.275102  0.211651  0.217793  0.300722  0.229410  0.327709  0.222091  0.193134  0.252512  0.200318  0.142252  0.092801  0.232188
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.166168 -0.015921  0.017057 -0.117601 -0.134931 -0.452250 -0.415719 -0.214474 -0.001877 -0.111159 -0.517580 -0.839848 -0.800957 -0.707663 -0.954208 -0.957566 -1.011020 -0.953123 -0.909063 -0.919021 -0.971058 -0.508728 -1.036910 -1.017100 -1.058017 -0.682883 -0.514492 -0.518338 -0.167146 -1.013457 -1.001532 -0.277552 -0.883820 -1.038184 -1.029708 -1.028291 -0.961813 -1.092902 -1.051965 -0.971640 -0.983561 -1.040172 -1.080627 -1.075328 -1.029863 -1.053317 -0.915655 -0.778892 -0.922337 -1.009312 -0.764499  0.061202 -0.047512  0.114908 -0.161317 -0.043258 -0.668248 -0.025059 -0.241160 -0.413536  0.092450 -0.145864 -0.216122 -0.011613
        -1.159958 -1.093360 -1.066789 -0.959882 -1.181083 -1.036607 -1.073862 -1.102965  1.041043  1.038591  0.172075 -0.033053 -0.039374 -0.025227  0.267213  0.313637  0.339086  0.274694  0.287272  0.421853  0.431494  0.979136 -0.474877 -0.344543 -0.157648 -0.091525 -1.000381 -1.077792 -1.074874 -0.473019 -0.539250 -1.170119 -0.734452 -0.391957 -0.284197 -0.298057 -0.033258  0.003050 -0.101160  0.324537  0.431740  0.262189 -0.250957 -0.260610 -0.297624 -0.434754  0.592989  0.126977  0.289536  0.384835 -0.083302  1.057807  1.088488  1.060776  1.052969  1.051970  0.091846  1.047566 -1.179467 -1.069250 -0.957312 -1.035902 -1.101053 -1.221383
        -0.108049 -0.259058 -0.306916 -0.442437 -0.013996  0.574491  0.517365 -0.153119  0.599719  0.594227  1.194833  0.931194  0.975590  1.069155  0.677328  0.592585  0.394825  0.675317  0.758808  0.642755  0.428900  0.427290  0.147117  0.535645  0.488228  1.084616  0.580302  0.267335 -0.250131  0.386666  0.351297  0.126116  0.362744  0.398832  0.536115  0.531797  0.675384  0.351818  0.544711  0.583673  0.327487  0.396268  0.318828  0.011870  0.526839  0.194237  0.266913  0.989141  0.731589  0.212701  1.013871  0.580291  0.434413  0.531916  0.533297  0.571776  1.098475  0.582514  0.151932  0.540623 -0.461514 -0.335085 -0.106057  0.166550
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.055528  0.074192  0.073360  0.059586  0.070170  0.065602  0.075350  0.059084  0.064690  0.063791  0.068762  0.066861  0.071862  0.055834  0.022396  0.022167  0.005953  0.017058  0.000269 -0.004344  0.004354 -0.007529  0.018520  0.029773  0.025030  0.025789  0.029198  0.045940  0.039489  0.034073  0.043235  0.034461  0.053565  0.045301  0.048803  0.046150  0.038534  0.056043  0.096326  0.099188  0.090936  0.071929  0.086351  0.090740  0.105892  0.092547  0.105610  0.110269  0.109069  0.127558  0.117889  0.117484  0.146376  0.172550  0.163473  0.172046  0.164004  0.206627  0.180903  0.185936  0.177548  0.160140  0.153000  0.164025  0.153678  0.154569  0.148671  0.128714  0.133842  0.126848  0.130990  0.149122  0.126733  0.157084  0.112609  0.121147  0.113961  0.111561  0.100300  0.106590  0.106844  0.111842  0.099683  0.080448  0.081519  0.095425  0.075421  0.128052  0.134131  0.117690  0.129150  0.121700  0.125936  0.136433  0.117850  0.142023  0.135353  0.169970  0.170936  0.186676  0.189525  0.134922  0.117962  0.101946  0.087272  0.111481  0.083848  0.115560  0.064362  0.022959  0.026334  0.029238  0.039408  0.036156  0.010617  0.013542  0.027625  0.033683 -0.001859  0.040731  0.041944  0.083396  0.083461  0.047345  0.051016  0.050510  0.075166  0.054794
        -0.015008 -0.014737 -0.049521 -0.076134 -0.086227 -0.129383 -0.136638 -0.151452 -0.173184 -0.173934 -0.192785 -0.204742 -0.209799 -0.211524 -0.183365 -0.167016 -0.147418 -0.133894 -0.121325 -0.099880 -0.074710 -0.143207 -0.020684 -0.059420 -0.054568 -0.085455 -0.102797 -0.130179 -0.136927 -0.124632 -0.075532 -0.062730 -0.024266  0.003655  0.022030  0.031490  0.052838  0.072890  0.083575  0.070064  0.045223  0.019906 -0.001562 -0.025260 -0.036294 -0.060111 -0.070681 -0.090723 -0.113950 -0.130922 -0.150664 -0.171275 -0.024153  0.027994  0.035117  0.053278  0.067379  0.019469 -0.202007 -0.203708 -0.200968 -0.204045 -0.208482 -0.207076 -0.209322 -0.208588 -0.210338  0.086549  0.071346  0.052737  0.026557  0.010707 -0.013183  0.035892  0.060857  0.087642  0.096307 -0.223092 -0.219994 -0.214528 -0.207911 -0.203286 -0.191142 -0.180935 -0.173976 -0.160118 -0.178413 -0.201710 -0.190417 -0.203103 -0.209059 -0.216236 -0.217354 -0.215446 -0.220253 -0.213742 -0.204852 -0.204805 -0.204304  0.052350  0.021567 -0.029076 -0.063289 -0.150163 -0.125830 -0.076963 -0.055856  0.005654  0.058225  0.037915 -0.005404 -0.013788  0.060116 -0.002118 -0.049610 -0.070843 -0.136805 -0.168115 -0.148186 -0.185996 -0.198651 -0.224426 -0.207243 -0.171034 -0.133520 -0.098077 -0.056609 -0.006722
         0.415362  0.432664  0.432594  0.422064  0.427653  0.432671  0.438778  0.419416  0.396556  0.391482  0.365245  0.340573  0.329858  0.303613  0.337394  0.356289  0.371417  0.384669  0.374415  0.370503  0.379128  0.365517  0.385524  0.396828  0.393849  0.393801  0.397310  0.415405  0.404528  0.401979  0.409232  0.399927  0.412621  0.408494  0.410819  0.405611  0.389077  0.370646  0.377004  0.392534  0.416358  0.423718  0.436894  0.443269  0.452365  0.442091  0.446165  0.448193  0.444851  0.443997  0.438059  0.431372  0.457810  0.437132  0.430637  0.388304  0.357570  0.440336  0.333125  0.359676  0.393759  0.395493  0.361400  0.352443  0.321005  0.273588  0.248744  0.354795  0.381057  0.410577  0.437055  0.453648  0.448807  0.430838  0.404036  0.358429  0.350001  0.263893  0.300201  0.316583  0.346422  0.367345  0.390995  0.400311  0.426937  0.433972  0.412642  0.375011  0.414429  0.370194  0.352871  0.311704  0.296662  0.256509  0.288336  0.292634  0.371162  0.388220  0.335848  0.389660  0.439921  0.455292  0.456006  0.437487  0.445714  0.447820  0.438491  0.441312  0.397901  0.386110  0.390895  0.395382  0.383347  0.400970  0.379809  0.384742  0.392430  0.367159  0.365247  0.349950  0.334665  0.284353  0.342694  0.377617  0.418701  0.420157  0.431126  0.415494
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.653047 -0.429601 -0.631634 -0.402250 -0.281618 -0.610898 -0.357097 -0.595925 -0.406230 -0.374821 -0.364752 -0.154578 -0.075262 -0.277805 -0.466471 -0.449369 -0.512190 -0.598197 -0.513793 -0.696962 -0.599633 -0.552773 -0.678151 -0.545380 -0.589161 -0.633001 -0.743616 -0.599973 -0.740744 -0.759669 -0.644664 -0.576807 -0.474134 -0.603761 -0.626912 -0.685017 -0.446449 -0.281066  0.138405  0.124245 -0.087351 -0.346044 -0.389284 -0.333224 -0.310276 -0.380155 -0.315470 -0.231975 -0.014229 -0.055360 -0.035193 -0.228573 -0.254381  0.415223  0.316579  0.184928  0.547960  0.293242  0.347291  0.630691  0.458263  0.130598  0.216696  0.312402  0.446547  0.526012  0.533938  0.453615  0.447805  0.361298 -0.097375 -0.289852 -0.003654  0.187132  0.116045  0.465817  0.292091  0.347269  0.190760  0.083234  0.046764  0.004709 -0.130544 -0.361141 -0.250275  0.200658 -0.395632 -0.007807 -0.147503  0.021337  0.018595  0.088741  0.286867  0.496969  0.330782  0.450233 -0.003514  0.387332  0.398132  0.354058  0.354196 -0.041316 -0.282843  0.121773 -0.377287 -0.275130 -0.456638  0.026439 -0.171177 -0.475876 -0.702819 -0.675504 -0.364790 -0.643186 -0.712865 -0.587274 -0.587948 -0.449460 -0.446457 -0.436463 -0.372942  0.085638 -0.046686 -0.383281 -0.555947 -0.517742 -0.537026 -0.597298
        -0.114056  0.242681 -0.376828  0.007897  0.049910 -0.250730 -0.305297 -0.437782 -1.039118 -1.036711 -0.920949 -1.100128 -1.146827 -1.108873 -0.842448 -0.885241 -0.681855 -0.256600 -0.140655 -0.054851  0.066558 -0.612057  0.107896 -0.088066 -0.113809 -0.081158 -0.094114 -0.326005 -0.403764 -0.282320  0.048549 -0.066887  0.051996  0.019523  0.120004  0.243242  0.539753  0.973301  0.907690  0.827635  0.684621  0.258668  0.159899  0.118131  0.050853 -0.325646 -0.248836 -0.082362 -0.163087 -0.290355 -0.412102 -0.486095  0.144388  0.811042  0.855376  0.994791  0.745462  0.936445 -1.174237 -1.090804 -1.152986 -1.160291 -1.199911 -1.195229 -1.165356 -1.136805 -1.132799  0.869576  0.926653  0.773114  0.593132  0.373695  0.321051  0.885045  0.828132  0.865374  0.961067 -1.193327 -1.175675 -1.197328 -1.176793 -1.153271 -1.081423 -1.076505 -0.862328 -0.499510 -1.038666 -1.086936 -1.106030 -1.131221 -1.188434 -1.211729 -1.196671 -1.148124 -1.180586 -1.164963 -1.125558 -1.159193 -1.173577  0.944529  0.899077  0.235168 -0.337812 -0.522334 -0.121878 -0.143712 -0.386956  0.327354  0.752019  0.191261  0.079537  0.069767  0.757539  0.061797  0.036583 -0.015516 -0.338608 -0.962912 -0.789005 -0.857058 -0.985358 -1.202133 -1.154866 -0.991771 -0.343793  0.049285 -0.327128 -0.061601
         1.114012  1.257241  1.061763  1.305040  1.353780  1.159049  1.324523  1.118686  0.565059  0.642948  0.896133  0.726900  0.646613  0.592491  0.908900  0.867959  1.039178  1.163389  1.231950  1.090401  1.163502  1.060419  1.091298  1.214372  1.180779  1.146116  1.031883  1.153273  0.970244  0.989438  1.124353  1.191340  1.258622  1.169064  1.141442  1.054114  1.115510  0.645634  0.941861  1.044805  1.164277  1.300240  1.301592  1.337525  1.357467  1.282355  1.335521  1.387767  1.436004  1.414111  1.391062  1.315190  1.358358  1.009797  0.981756  0.725033  0.984292  0.829760  0.168557  0.483178  0.515663  0.686561  0.475464  0.313222  0.336075  0.252556  0.223560  0.885180  0.793219  1.063333  1.228789  1.275561  1.382234  0.958129  1.045345  0.884106  0.819533  0.319458  0.580364  0.499773  0.586541  0.660691  0.783927  0.454656  1.071983  1.368712  0.607767  0.819891  0.701370  0.729796  0.521992  0.400624  0.419299  0.245338  0.468392  0.279006  0.735876  0.592908  0.241547  0.809183  0.890321  1.403509  1.330073  1.365439  1.331248  1.367644  1.211956  1.383950  1.078467  1.239133  1.069954  1.099908  0.975145  1.132003  1.064750  1.182212  1.154234  0.734404  0.991093  0.917367  0.791788  0.494311  0.634369  0.753054  1.185110  1.229708  1.174120  1.171645
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.912693 -0.242616 0.328830]';
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.134603 0.959645 0.246908]';
q1a = [-0.039071 -0.021976 0.130688]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.163397 0.267260 -0.949670]';
q1b = [-0.039071 -0.021976 0.130688]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [-0.977335 0.087484 0.192777]';
q1c = [-0.039071 -0.021976 0.130688]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [-0.265878 0.915062 0.303267]';
q2a = [-0.073871 0.034944 -0.071569]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.330813 0.208879 -0.920289]';
q2b = [-0.073871 0.034944 -0.071569]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [-0.905468 -0.345010 0.247178]';
q2c = [-0.073871 0.034944 -0.071569]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.026383 0.976862 0.212235]';
q3a = [-0.132058 0.071684 -0.233441]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [-0.589389 0.186683 -0.785983]';
q3b = [-0.132058 0.071684 -0.233441]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [-0.807418 -0.104353 0.580677]';
q3c = [-0.132058 0.071684 -0.233441]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [-0.034875 0.966146 0.255626]';
q4a = [-0.182555 0.087679 -0.300782]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [0.670875 0.212218 -0.710557]';
q4b = [-0.182555 0.087679 -0.300782]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [-0.740750 0.146712 -0.655564]';
q4c = [-0.182555 0.087679 -0.300782]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [-0.134603 -0.959645 -0.246908]';
q5a = [-0.039071 -0.021976 0.130688]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.278356 -0.275761 0.920040]';
q5b = [-0.039071 -0.021976 0.130688]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [-0.951000 0.055112 0.304241]';
q5c = [-0.039071 -0.021976 0.130688]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [0.073919 -0.948907 -0.306777]';
q6a = [0.012602 -0.073168 0.301482]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [0.815108 -0.119747 0.566798]';
q6b = [0.012602 -0.073168 0.301482]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [-0.574574 -0.291953 0.764609]';
q6c = [0.012602 -0.073168 0.301482]';
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
