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 N x 1 angles, where N is the number of rotation axes.
%%% Outputs %%%
% new_config: a point cloud from the surface of the kinematic model in the new configuration.
% new_ctrm: a new center of mass estimate for the kinematic model.

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

%%% Sampled meshes for each segment
I1  = [  0.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.029024 -0.016186 -0.025193 -0.011391 -0.012581 -0.015031 -0.067768 -0.062116 -0.055906 -0.046544 -0.051154 -0.054261 -0.055780 -0.047473 -0.068086 -0.061573 -0.044005 -0.068182 -0.083677 -0.103554 -0.099424 -0.080218 -0.137202 -0.123062 -0.118731 -0.119006 -0.151694 -0.141586 -0.132284 -0.150698 -0.147536 -0.143683 -0.127860 -0.127685 -0.120651 -0.125199 -0.127126 -0.126599 -0.166390 -0.169093 -0.174534 -0.170760 -0.179216 -0.189133 -0.179691 -0.172695 -0.177979 -0.174272 -0.182888 -0.182502 -0.166189 -0.167813 -0.179244 -0.186495 -0.182488 -0.172938 -0.204851 -0.198555 -0.199646 -0.190556 -0.187341 -0.199397 -0.201740 -0.210796 -0.201369 -0.222546 -0.220360 -0.222374 -0.224296 -0.215701 -0.200461 -0.213318 -0.199337 -0.226944 -0.206082 -0.209026 -0.220952 -0.214755 -0.206441 -0.207801 -0.198052 -0.192964 -0.212708 -0.222340 -0.218145 -0.221033 -0.208757 -0.207009 -0.235358 -0.242192 -0.234932 -0.229581 -0.232617 -0.232497 -0.233662 -0.251010 -0.252389 -0.237547 -0.258420 -0.244861 -0.241330 -0.260088 -0.269238 -0.285238 -0.295739 -0.309481 -0.303967 -0.308176 -0.301171 -0.304535 -0.300818 -0.291005 -0.305533 -0.316213 -0.299933 -0.295082 -0.294809 -0.307114 -0.294762 -0.295403 -0.337656 -0.345210 -0.347626 -0.341769 -0.316549 -0.333899 -0.328464 -0.325292 -0.323623 -0.278470 -0.277042 -0.273272 -0.266738 -0.260759 -0.264195 -0.267756 -0.267313 -0.274061 -0.260545 -0.271778 -0.266205 -0.265983 -0.262022 -0.257535 -0.259488 -0.277848 -0.256124 -0.256137 -0.265558 -0.265208 -0.262488 -0.280918 -0.237772 -0.237426 -0.236877 -0.242646 -0.249774 -0.240548 -0.230915 -0.222658 -0.238898 -0.247501 -0.243763 -0.253818 -0.239955 -0.228674 -0.254603 -0.244456 -0.252830 -0.246241 -0.277708 -0.258739 -0.248209 -0.285522 -0.284374 -0.298232 -0.279218 -0.284924 -0.284577 -0.236849 -0.232612 -0.264238 -0.229377 -0.236787 -0.212390 -0.219777 -0.206563 -0.212768 -0.218025 -0.248763 -0.226878 -0.231711 -0.224499 -0.189638 -0.193814 -0.188030 -0.233108 -0.191088 -0.190775 -0.227117 -0.222286 -0.221883 -0.210972 -0.176011 -0.208813 -0.161087 -0.187247 -0.145028 -0.156539 -0.158188 -0.156535 -0.143126 -0.171075 -0.182504 -0.162889 -0.199221 -0.192315 -0.197947 -0.172628 -0.094169 -0.095632 -0.096452 -0.097142 -0.091539 -0.097052 -0.089560 -0.080645 -0.078228 -0.078826 -0.111004 -0.125123 -0.118672 -0.125270 -0.069228 -0.073900 -0.108817 -0.118899 -0.109639 -0.105702 -0.114557 -0.139059 -0.144213 -0.146702 -0.125158 -0.142103 -0.141028 -0.071466 -0.070177 -0.036621 -0.041145 -0.076503 -0.038878 -0.016568 -0.037219 -0.044268 -0.035384
         0.081555  0.102199  0.131796  0.137744  0.151365  0.165172  0.200490  0.201551  0.199114  0.185039  0.170999  0.151770  0.145433  0.134191  0.098313  0.077993  0.073921  0.040300  0.063531  0.037122  0.015875  0.002986 -0.011927 -0.007195  0.014149  0.028144  0.028755  0.060815  0.083794  0.100459  0.127370  0.125749  0.137677  0.157444  0.180636  0.204020  0.202731  0.198894 -0.005545 -0.002900  0.008925  0.021349  0.027221  0.037640  0.058775  0.086986  0.091364  0.103872  0.134738  0.149938  0.152332  0.167667  0.175549  0.170940  0.173267  0.172725  0.101509  0.009396  0.006745  0.000792 -0.000410  0.003417  0.004388  0.013331  0.031523  0.038181  0.054818  0.075498  0.090467  0.116976  0.117849  0.146758  0.146737  0.181918  0.180362  0.173563  0.162589  0.152608  0.148187  0.152219  0.164231  0.167805  0.127303  0.098150  0.089826  0.046108  0.069144  0.035039  0.033950  0.017946  0.009124  0.004241 -0.005187 -0.007831  0.002902  0.019875  0.049002  0.064122  0.090284  0.102796  0.114238  0.118579  0.107588  0.125180  0.175392  0.176285  0.169632  0.159978  0.134998  0.098797  0.090593  0.072824  0.059627  0.054023  0.052923  0.033047  0.017449  0.010327  0.006724  0.007859  0.121753  0.082437  0.060937  0.027174  0.015288  0.032766  0.037315  0.107864  0.148178  0.066940  0.041326  0.014297  0.002634  0.004208  0.013064  0.018211  0.027210  0.038029  0.046446  0.073695  0.098580  0.117228  0.125059  0.149801  0.165334  0.182220  0.184972  0.185795  0.171334  0.165512  0.143537  0.158126  0.146945  0.131948  0.166329  0.180829  0.183037  0.182255  0.186313  0.187167  0.176467  0.149543  0.145659  0.111337  0.091753  0.133662  0.142027  0.088100  0.072774  0.054097  0.050967  0.031023  0.011648  0.010058  0.023135  0.059928  0.129548  0.161873  0.181663  0.115392  0.080017  0.085196  0.045939  0.021714 -0.003242 -0.007910 -0.007240 -0.001274  0.008890  0.004275  0.006175  0.068936  0.138890  0.167799  0.172857  0.159712  0.182148  0.139551  0.129638  0.111711  0.066733  0.061571  0.002444 -0.003412  0.077512  0.177975  0.169644  0.186149  0.190420  0.191355  0.179179  0.150741  0.178542  0.162034  0.100445  0.072565  0.050709  0.023806 -0.005851  0.208131  0.207563  0.189745  0.182356  0.157891  0.132189  0.116149  0.101790  0.099955  0.081343  0.075102  0.087697  0.112122  0.119149  0.139522  0.157558  0.153827  0.168934  0.198241  0.206178  0.207585  0.191728  0.195449  0.142453  0.098088  0.011912 -0.007145  0.009341  0.079343  0.156385  0.185406  0.195648  0.178325  0.151235  0.149934  0.123395  0.071773
        -0.444425 -0.467878 -0.481267 -0.474030 -0.460702 -0.448268 -0.392742 -0.414837 -0.418733 -0.433131 -0.458423 -0.468187 -0.469524 -0.477296 -0.462767 -0.437332 -0.433093 -0.422698 -0.430881 -0.413332 -0.400152 -0.383339 -0.338107 -0.355991 -0.391849 -0.401041 -0.382589 -0.407730 -0.419947 -0.397490 -0.393102 -0.398800 -0.411411 -0.404524 -0.398943 -0.380494 -0.352891 -0.322986 -0.326681 -0.330718 -0.372003 -0.382588 -0.389285 -0.399107 -0.400709 -0.406501 -0.410580 -0.405069 -0.399145 -0.388587 -0.374050 -0.361805 -0.335811 -0.313275 -0.285882 -0.268225 -0.269024 -0.300526 -0.305228 -0.300225 -0.353020 -0.390079 -0.394967 -0.416995 -0.405163 -0.436203 -0.425355 -0.428841 -0.433844 -0.432564 -0.421436 -0.419916 -0.406046 -0.397863 -0.382277 -0.352323 -0.324918 -0.306074 -0.289236 -0.262144 -0.269525 -0.277607 -0.276322 -0.294851 -0.290804 -0.294800 -0.283204 -0.292195 -0.305867 -0.335426 -0.341673 -0.340627 -0.381702 -0.402262 -0.423972 -0.441627 -0.459406 -0.454187 -0.470418 -0.455931 -0.449560 -0.462034 -0.467257 -0.461147 -0.431261 -0.398840 -0.370345 -0.357084 -0.338460 -0.326683 -0.322793 -0.314052 -0.319551 -0.321884 -0.316539 -0.324823 -0.352135 -0.379787 -0.391690 -0.427137 -0.347865 -0.327757 -0.323032 -0.336248 -0.350893 -0.331634 -0.328998 -0.340573 -0.355108 -0.477784 -0.462247 -0.437312 -0.415256 -0.389649 -0.366290 -0.355045 -0.331703 -0.314240 -0.303491 -0.308856 -0.319903 -0.326136 -0.325931 -0.328412 -0.339986 -0.391566 -0.403498 -0.418006 -0.434730 -0.438175 -0.448963 -0.441804 -0.439235 -0.445425 -0.428306 -0.416610 -0.419886 -0.401552 -0.380479 -0.392529 -0.347477 -0.321743 -0.318098 -0.318149 -0.304278 -0.305201 -0.324190 -0.307435 -0.306941 -0.301332 -0.307585 -0.323262 -0.355454 -0.377580 -0.346120 -0.316052 -0.334879 -0.354701 -0.418302 -0.446415 -0.441401 -0.472683 -0.444541 -0.440283 -0.400609 -0.370303 -0.351218 -0.322952 -0.317838 -0.375615 -0.333993 -0.297129 -0.301398 -0.313869 -0.367974 -0.389249 -0.400438 -0.404489 -0.411211 -0.439301 -0.426658 -0.426009 -0.410249 -0.323367 -0.283506 -0.268773 -0.304370 -0.283305 -0.338275 -0.345924 -0.363845 -0.391224 -0.346801 -0.380106 -0.398842 -0.413337 -0.403904 -0.400705 -0.319301 -0.368889 -0.376402 -0.404135 -0.406667 -0.423641 -0.431643 -0.446418 -0.462781 -0.463212 -0.440115 -0.432254 -0.425247 -0.422554 -0.417344 -0.459356 -0.441263 -0.414810 -0.407430 -0.399689 -0.385799 -0.363631 -0.303577 -0.347240 -0.389380 -0.421838 -0.368820 -0.347598 -0.400289 -0.438863 -0.473228 -0.431581 -0.408972 -0.443405 -0.478421 -0.475281 -0.479966 -0.430836
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.113888  0.201164  0.185654  0.599571  0.896594  0.718114  0.057196 -0.435752 -0.227174 -0.008544 -0.594217 -0.495717 -0.542040 -0.422808 -0.150417 -0.008392 -0.061764 -0.123941 -0.102540 -0.353966 -0.260103 -0.188018 -0.304540 -0.185286 -0.707545 -0.600452 -0.131864 -0.796315 -0.769495 -0.569574 -0.862335 -0.946904 -0.607050 -0.730179 -0.575788 -0.578776 -0.477050 -0.339118 -0.505992 -0.421709  0.026742  0.085103  0.312677  0.324321  0.091952  0.313836  0.355713  0.484792  0.508512  0.540667 -0.183470 -0.635900 -0.910429 -0.837290 -0.507066 -0.539168 -1.119327 -0.868446 -0.767271 -0.864907  0.141275  0.463135  0.532163  0.639567  0.331005  0.591064  0.605086  0.371021  0.359755  0.342663  0.291086  0.452434  0.355221 -0.222310  0.296013  0.095857 -0.172762 -0.118156 -1.026772 -1.089544 -0.728964 -0.697880 -1.210707 -0.721564 -0.727981 -0.626645 -0.768985 -0.780217 -0.521855 -0.702937 -0.846036 -0.886425 -0.551315 -0.232585 -0.271306 -0.155912  0.117184  0.459420  0.081006  0.568965  0.350147 -0.097410 -0.140861 -0.022415 -0.193471 -0.376844 -0.624937 -0.568452 -0.305212 -0.802073 -0.791638 -0.727120 -0.651337 -0.589936 -0.667230 -0.481435 -0.094226 -0.260380 -0.272084 -0.233235 -0.443612 -0.243697 -0.419290 -0.373793 -0.462780 -0.393050 -0.454632 -0.600185 -0.775015 -0.269705 -0.165547 -0.261537 -0.361943 -0.513013 -0.456539 -0.399207 -0.379386 -0.382314 -0.445445 -0.316322 -0.481458 -0.626537 -0.777702 -0.844719 -0.996574 -0.330358 -0.084689 -0.053822 -0.227474 -0.218812 -0.183776 -0.225002  0.216785  0.243205  0.205611  0.134141  0.099520 -0.125666 -0.514698 -0.286263 -0.318224 -0.750544 -0.745621 -0.822035 -0.874206 -0.895938 -0.804928 -0.752835 -0.479289 -0.446407 -0.637141 -0.344844 -0.708922 -0.253625 -0.211898 -0.736666 -0.477992 -0.381880 -0.483839  0.326193  0.623994  0.010511  0.502220  0.038125  0.234998 -0.318855  0.021868 -0.707080 -0.803019 -0.685956 -0.881613 -0.728608 -0.916707 -0.899353  0.264503  0.492171 -0.148851  0.406729  0.343919  0.346179  0.468740  0.562070  0.503922 -0.435948 -0.821074 -0.529796 -0.826905 -0.416070 -0.399289 -0.530483 -0.817032 -0.835024 -0.870356  0.527440  0.171398  0.274109  0.308495  0.329134 -0.525344  0.085165  0.065080 -0.255351 -0.390712 -0.658071 -0.658336 -0.801387 -0.853291 -0.394835 -0.178496 -0.376235 -0.712694 -0.641513 -0.621618 -0.829508 -0.781786 -0.530787 -0.521296 -0.311757 -0.037374 -0.319041 -0.454426 -0.444586 -0.812063 -0.692601 -0.322598 -0.499592 -0.289767 -0.068434 -0.298552  0.142303 -0.558521  0.054370  0.136978 -0.267305 -0.236415 -0.030648
        -0.749125 -0.511698  0.252540  0.057584  0.544822  0.724629  1.173579  0.810016  0.747438  0.980473  0.838957  0.476974  0.390772  0.366698 -0.659240 -0.583158 -0.617593 -0.394933 -0.329939 -0.464070 -0.651196 -0.929837 -0.953673 -0.919026 -0.522180 -0.477218 -0.423731 -0.242859  0.170291  0.804744  0.279216  0.291137  0.518053  0.334113  0.526736  1.072643  1.135056  1.149998 -0.876392 -0.919511 -0.832225 -0.454908 -0.340483 -0.159802 -0.255610  0.042906  0.012594  0.346495  0.536155  0.526979  0.624098  0.762947  0.792507  0.925171  1.109623  1.090013 -0.202062 -0.575447 -0.690028 -0.722370 -0.938149 -0.618086 -0.525663 -0.155868 -0.169873  0.102247  0.180453 -0.111983 -0.065726  0.062508  0.262033  0.513050  0.581328  1.036603  1.040625  1.049054  1.017732  0.903601  0.718480  0.621789  1.000828  1.028433  0.099234  0.256715  0.225158 -0.275502  0.062917 -0.401305 -0.590120 -0.754679 -0.659426 -0.632586 -0.944655 -1.007815 -0.691977 -0.627275 -0.315728  0.022645  0.216130  0.333141  0.158384  0.583738  0.559873  0.522852  1.005056  1.149327  1.037636  0.903224  0.562482  0.301712  0.269331  0.227209  0.224293  0.120672  0.048305 -0.829586 -0.840850 -0.931813 -0.952661 -0.910068  0.543760  0.474105 -0.178493 -0.707886 -0.902357 -0.475626 -0.361460  0.404765  0.704285  0.071350 -0.564603 -0.760811 -0.970021 -0.907467 -0.911922 -0.897252 -0.916671 -0.623252 -0.307289  0.479344  0.466104  0.377753  0.294098  0.418742  0.759914  1.126339  1.134103  1.113651  0.728233  0.639425  0.584314  0.594218  0.557193  0.337032  0.625216  1.024326  1.021381  1.175621  1.144988  1.098193  1.105253  0.437536  0.391788  0.286373  0.090466  0.262037  0.350199  0.198595  0.354262  0.068632 -0.206436 -0.838055 -0.804873 -0.965091 -0.836514  0.149601  0.534408  0.862529  1.133551  0.102508  0.187320  0.139372 -0.001953 -0.526536 -0.838387 -1.001172 -0.979823 -0.778740 -0.539978 -0.861557 -0.607856  0.109544  0.274617  0.871420  1.040793  0.572640  1.153120  0.620863  0.513377  0.028741 -0.026248  0.112620 -0.560373 -0.934376  0.088632  1.096953  0.938294  1.131473  1.138105  1.119258  0.559918  0.377588  0.755223  0.687614  0.537224 -0.157129 -0.140266 -0.215492 -0.891935  1.191876  1.178839  0.589141  0.521230  0.501293  0.444597  0.379427 -0.200966 -0.629063 -0.502927 -0.197570  0.259512  0.298621  0.291827  0.385607  0.563468  0.415846  0.546561  0.561424  1.167100  1.185619  1.128754  1.136376  0.329111  0.306563 -0.708712 -0.822279 -0.803555 -0.550970  0.507055  0.986179  0.723974  1.088134  0.242093  0.410867  0.130311 -0.644382
        -0.994669 -1.226751 -1.451300 -1.261621 -0.597051 -0.834929 -0.199141 -1.114956 -1.237273 -1.037978 -0.967260 -1.304463 -1.308190 -1.374550 -1.110329 -1.185698 -1.155238 -1.321288 -1.350025 -1.241644 -1.127611 -0.727170 -0.629848 -0.761816 -0.996539 -1.117155 -1.274140 -1.099909 -1.185767 -0.970662 -1.075746 -0.971061 -1.202417 -1.182849 -1.219348 -0.579896 -0.261653 -0.098848 -0.682063 -0.640574 -0.873955 -1.223846 -1.178485 -1.234201 -1.310309 -1.279052 -1.252582 -1.116946 -0.999837 -0.967263 -1.255605 -1.014902 -0.626897 -0.226083 -0.152144 -0.111838 -0.001484  0.156862  0.133304 -0.150936 -0.465520 -0.808150 -0.819741 -0.915247 -1.227366 -1.014073 -0.975678 -1.211613 -1.230391 -1.260370 -1.280269 -1.069996 -1.115691 -0.916990 -0.470862  0.022594  0.191230  0.347131 -0.246545 -0.217035 -0.134476 -0.211431 -0.219661  0.556990  0.558757  0.560797  0.545112  0.401373  0.421083  0.101340  0.082111  0.063115 -0.255872 -0.396527 -1.142112 -1.198034 -1.314033 -1.170089 -1.402595 -0.989436 -1.254672 -1.332185 -1.349843 -1.340226 -0.980076 -0.178267  0.008531  0.259229  0.565554  0.516692  0.529937  0.572497  0.604194  0.637613  0.613553  0.145429  0.120189 -0.047839 -0.110504 -0.818988  0.552514  0.586733  0.645234  0.340997  0.018626  0.527458  0.579330  0.573912  0.342739 -1.477736 -1.249914 -1.069032 -0.627102 -0.065278 -0.038562  0.025281 -0.021014  0.428046  0.613396  0.604087  0.584858  0.569479  0.514009  0.435057 -0.018770 -0.066172 -0.139378 -0.731299 -1.264436 -1.317524 -1.343122 -1.339967 -1.229852 -1.298030 -1.198778 -0.799423 -0.838422 -0.406652 -0.360256 -0.799942  0.014551  0.493124  0.511094  0.485898  0.468848  0.428473  0.484393  0.546580  0.625518  0.678322  0.589383  0.163862 -0.007551 -0.158334  0.159599  0.578227  0.557633  0.352180 -0.651120 -1.272743 -0.946091 -1.432672 -1.124594 -1.230121 -0.720572 -0.310175 -0.307622  0.065896  0.279260 -0.132313  0.106788  0.569730  0.407422 -0.278913 -0.176575 -0.994903 -0.624299 -1.045545 -1.164103 -1.254438 -1.143482 -1.044727 -0.825176 -0.579766  0.507071 -0.129009 -0.300474 -0.101927 -0.130990 -0.326138 -1.010979 -1.076656 -0.769227 -0.848875 -1.233993 -1.263748 -1.248130 -1.216132 -0.620043 -0.378129 -0.551884 -1.307038 -1.300524 -1.172718 -1.198038 -1.097560 -1.020774 -1.070083 -1.245531 -1.357080 -1.215969 -1.254332 -1.268121 -1.060535 -1.019306 -1.282731 -1.241380 -1.309220 -0.654050 -0.323182 -0.153487 -0.190935 -1.112190 -1.218422 -1.038239 -0.802558 -0.940238 -1.215202 -1.372351 -1.001764 -1.107922 -0.847821 -1.462376 -1.412817 -1.461308 -1.128761
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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
