function [new_config, new_ctrm, new_norms]  = jdl512_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.003687 -0.000035  0.008114 -0.049632 -0.035492 -0.063182 -0.048523 -0.036498 -0.045104 -0.061197 -0.058283 -0.096804 -0.103926 -0.091403 -0.106739 -0.110492 -0.125033 -0.147317 -0.143059 -0.140414 -0.146211 -0.147278 -0.144943 -0.140974 -0.130428 -0.124973 -0.042347 -0.031834 -0.020478 -0.020529 -0.036416 -0.032555 -0.008877 -0.013625 -0.036096 -0.027970 -0.022264 -0.010832 -0.021284 -0.017283 -0.007537 -0.017401 -0.027837 -0.059128 -0.088697 -0.065729 -0.076105 -0.060840 -0.066756 -0.088567 -0.076659 -0.059439 -0.097388 -0.099646 -0.114699 -0.125405 -0.118704 -0.122515 -0.129399 -0.128943 -0.124008 -0.122199 -0.086994 -0.152943 -0.145392 -0.142898 -0.144593 -0.144851 -0.136983 -0.137513 -0.142292 -0.135536 -0.143981 -0.099816 -0.135561 -0.114128 -0.132890 -0.053953 -0.096000 -0.052709 -0.092220 -0.015063 -0.048360 -0.053089 -0.051403 -0.006944 -0.088315 -0.095278 -0.098154 -0.096344 -0.072720 -0.089884 -0.109228 -0.125542 -0.129438 -0.127295 -0.123852 -0.141153 -0.142199 -0.138476 -0.133325 -0.133027 -0.126976 -0.137594 -0.143410 -0.147483 -0.147378 -0.144684 -0.134672 -0.139608 -0.145597 -0.115499 -0.135935 -0.109705 -0.073066 -0.078341 -0.144030 -0.147698 -0.146222 -0.141230 -0.138111 -0.111364 -0.095097 -0.071582 -0.031566 -0.033258  0.000102 -0.002423
        -0.198723 -0.153785 -0.137511  0.123250  0.128402  0.113406  0.086477  0.082299  0.075965  0.045913  0.035369  0.009694  0.017513  0.034226  0.025568 -0.006069 -0.010197 -0.054885 -0.072045 -0.050487 -0.077626 -0.055049 -0.072853 -0.050354 -0.065365 -0.041019 -0.132703 -0.146865 -0.149805 -0.171342 -0.178993 -0.197760 -0.211777 -0.178543 -0.232307 -0.254477 -0.225634 -0.216543 -0.271624  0.110400  0.136382  0.151014  0.060288 -0.238653 -0.201236 -0.200666 -0.169257 -0.170921 -0.153657 -0.137017 -0.125242 -0.128357 -0.099859 -0.109868 -0.094576 -0.087934 -0.125537 -0.133229 -0.148235 -0.154479 -0.167581 -0.176554 -0.223470 -0.093265 -0.122761 -0.099549 -0.117747 -0.095459 -0.120114 -0.098211 -0.144368 -0.159375 -0.139921 -0.175482 -0.149580 -0.145277 -0.112455 -0.124015 -0.138570 -0.187315 -0.193002 -0.261666 -0.218168 -0.173247 -0.142298 -0.141189  0.107626  0.081483  0.094090  0.073955  0.064322  0.052536  0.049030  0.037173  0.050326  0.062797  0.076093  0.025994  0.026187  0.038643  0.021962 -0.015665 -0.005279 -0.030273 -0.032770 -0.010174 -0.027777 -0.005146 -0.020438  0.003423  0.003530  0.081907  0.043531  0.072811  0.102203  0.132573 -0.041623 -0.041454 -0.086387 -0.085969 -0.035505  0.004365  0.046339  0.032917  0.069508  0.117011 -0.141935 -0.215799
         0.055488  0.079090  0.100874 -0.124804 -0.131693 -0.123243 -0.167292 -0.183218 -0.181746 -0.213651 -0.227151 -0.207145 -0.189494 -0.181847 -0.149011 -0.148944 -0.165066 -0.143208 -0.116837 -0.093178 -0.054379 -0.031454 -0.001705  0.019061  0.041609  0.047818  0.090346  0.071941  0.075317  0.048641  0.029125  0.022547  0.034677  0.050933 -0.004920 -0.014301  0.012672  0.029334 -0.020881 -0.166056 -0.135384 -0.105041 -0.219854 -0.031113 -0.032267 -0.010309  0.004177  0.015589  0.033038  0.048219  0.072462  0.082717  0.078044  0.068075  0.061376  0.049601  0.039940  0.025551 -0.018094 -0.026004 -0.034623 -0.043151 -0.046364 -0.123613 -0.100110 -0.079152 -0.046662 -0.023974  0.008545  0.029194 -0.056602 -0.040754 -0.092275 -0.022879 -0.030096  0.013274  0.033912  0.093998  0.040410  0.010628 -0.029624 -0.008047 -0.006187  0.019536  0.064809  0.098375 -0.101715 -0.128285 -0.103133 -0.138339 -0.168076 -0.159842 -0.137350 -0.116105 -0.083659 -0.057584 -0.035825 -0.014886 -0.021887 -0.048371 -0.104446 -0.124153 -0.130033 -0.115233 -0.078015 -0.053708 -0.022899 -0.001637  0.024847 -0.090823 -0.013002 -0.070144 -0.058729 -0.115088 -0.126617 -0.081265  0.008752 -0.034467 -0.042664 -0.090619 -0.168919 -0.168317 -0.158876 -0.215288 -0.202970 -0.145947  0.098724  0.038097
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.562700 -0.057806  0.323800 -0.574118 -0.481618 -0.591756 -0.580563 -0.576390 -0.575335 -0.600934 -0.679831 -0.713433 -0.751242 -0.919813 -0.936804 -0.831891 -0.890828 -0.978639 -1.009627 -1.128910 -1.141859 -1.146254 -1.138394 -1.077247 -0.960406 -1.066648 -0.439623 -0.396292 -0.366817 -0.588190 -0.647115 -0.634019 -0.623457 -0.645478 -0.637194 -0.609195 -0.627088 -0.612718 -0.625421 -0.485467 -0.179688 -0.316860 -0.344710 -0.662354 -0.697919 -0.676697 -0.590392 -0.568876 -0.379978 -0.425072 -0.543329 -0.484466 -0.722881 -0.672500 -0.866403 -0.896745 -0.651484 -0.713993 -0.821122 -0.776077 -0.674614 -0.731723 -0.701953 -1.116364 -1.109914 -1.138703 -1.141376 -1.139023 -1.068458 -1.074802 -1.125692 -0.972213 -1.130281 -0.640437 -0.943047 -0.555545 -0.953607 -0.522309 -0.459190 -0.661091 -0.701389 -0.599516 -0.656255 -0.578349 -0.410694 -0.134725 -0.770258 -0.703952 -0.823682 -0.745103 -0.672414 -0.764331 -0.890139 -1.005495 -1.022907 -1.023405 -1.029558 -1.081628 -1.104594 -1.077185 -1.047249 -1.046727 -0.888528 -1.120707 -1.114999 -1.145579 -1.146581 -1.115587 -0.974886 -1.080815 -1.132577 -0.979427 -1.060257 -0.891033 -0.644285 -0.715369 -1.108943 -1.147094 -1.144894 -1.137637 -1.018440 -0.714027 -0.821208 -0.707054 -0.481660 -0.535240  0.016483 -0.609263
        -0.630259 -0.906064 -0.818448  0.755332  0.844945  0.759948  0.705189  0.703938  0.714509  0.693010  0.539883  0.501028  0.775166  0.563471  0.001344  0.686032  0.598770  0.420711  0.248232 -0.056539 -0.127622 -0.086717 -0.131477 -0.007549  0.009658  0.009486 -0.932464 -0.961235 -0.945610 -0.608252 -0.541553 -0.620260 -0.680833 -0.544160 -0.703761 -0.746434 -0.695311 -0.681089 -0.716489  0.778021  1.032005  0.933250  0.868746 -0.715650 -0.666180 -0.654825 -0.806745 -0.733958 -0.973524 -0.918896 -0.834872 -0.865737 -0.562811 -0.602013 -0.342281 -0.140746 -0.742990 -0.842263 -0.768817 -0.702175 -0.789962 -0.774390 -0.688087 -0.030294 -0.099721 -0.067798 -0.188657 -0.164955 -0.460880 -0.192509 -0.318145 -0.626483 -0.173550 -0.731873 -0.544979 -0.944005 -0.404917 -0.847144 -0.925730 -0.640987 -0.664772 -0.752255 -0.679184 -0.662163 -0.978967 -0.997330  0.716213  0.756876  0.688519  0.624448  0.615848  0.568502  0.374074  0.230092  0.476797  0.490564  0.486830  0.315381  0.285013  0.380985  0.127059  0.193411  0.236511  0.066672 -0.029885  0.038503 -0.016813  0.095839  0.087587  0.021877  0.122970  0.559796  0.413473  0.614682  0.715425  0.806417 -0.008084 -0.054093 -0.137759 -0.013589  0.384453  0.704469  0.521940  0.506122  0.791182  0.728741 -0.993131 -0.686636
         0.757615  0.735397  0.761682 -0.695231 -0.667918 -0.673328 -0.745331 -0.750740 -0.739523 -0.752120 -0.826437 -0.822255 -0.272816 -0.365251 -0.706151 -0.124855  0.041615  0.144391  0.265893 -0.244305 -0.133034  0.000838  0.096373  0.367717  0.594337  0.380532  0.540406  0.523575  0.572051  0.746403  0.733111  0.700584  0.668930  0.734370  0.640380  0.633893  0.655784  0.678896  0.640265 -0.744923 -0.545535 -0.651329 -0.715824  0.608098  0.610132  0.638118  0.577865  0.667424  0.512319  0.573032  0.600330  0.607714  0.706083  0.723538  0.672591  0.683828  0.618643  0.409510  0.351226  0.504324  0.521681  0.477495  0.590784  0.136876  0.162884 -0.164957 -0.009312  0.058433  0.135951  0.364747 -0.004381  0.245219  0.069221  0.608118  0.407655  0.422154  0.524717  0.601666  0.538879  0.661821  0.607829  0.638279  0.640287  0.716001  0.478204  0.599068 -0.507393 -0.544651 -0.449922 -0.663763 -0.747891 -0.687952 -0.670759 -0.550231 -0.224493 -0.175913 -0.140668  0.163334  0.060613 -0.088718 -0.496489 -0.472635 -0.731346 -0.270439 -0.314670 -0.091596  0.015468  0.215504  0.556234 -0.428149  0.094719 -0.229053 -0.152392 -0.424769 -0.672206 -0.455603  0.269168 -0.002109 -0.046634 -0.134609 -0.389502  0.214634 -0.655430 -0.825413 -0.728900 -0.757339  0.623316  0.678455
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.021569  0.017349  0.011594  0.007544  0.012640  0.004606  0.018351 -0.018235  0.002582 -0.018991 -0.076738 -0.071308 -0.064848 -0.054820 -0.058598 -0.058679 -0.049143 -0.052801 -0.031995 -0.024601 -0.109287 -0.097745 -0.090081 -0.088420 -0.081837 -0.064198 -0.064313 -0.065646 -0.057467 -0.040386 -0.026654 -0.023309 -0.021186 -0.017459 -0.032077 -0.008900 -0.000235 -0.084210 -0.082878 -0.075781 -0.075470 -0.047159 -0.089352 -0.090706 -0.092691 -0.098717 -0.089016 -0.060221 -0.047750 -0.085276 -0.090080 -0.108359 -0.084651 -0.085925 -0.087649 -0.088327 -0.103490  0.004920 -0.005878 -0.062943 -0.087375 -0.005782 -0.000977  0.013259
         0.254560 -0.056242 -0.021084  0.007259  0.033425  0.270884  0.262278  0.251540  0.262099  0.266200  0.193686  0.195181  0.191161  0.208316  0.218806  0.226089  0.235748  0.228762  0.239589  0.233003  0.067640  0.079584  0.079837  0.084535  0.093095  0.070927  0.047378  0.023291 -0.004968 -0.015173 -0.038436 -0.023836  0.004762  0.011496  0.050270  0.057170  0.020511  0.003300  0.033404  0.003702  0.035138  0.031823  0.115117  0.107711  0.107328  0.117091  0.130160  0.178881  0.204079  0.146848  0.139574  0.090372  0.169607  0.170326  0.135562  0.050483  0.048159  0.235878  0.232646  0.192759  0.163128  0.269981 -0.046383  0.254061
         0.024205  0.195504  0.214043  0.240787  0.274722  0.123061  0.066801  0.054612  0.084173  0.114617  0.147664  0.110875  0.063979  0.056309  0.103516  0.142755  0.101215  0.099031  0.035783 -0.010113  0.093070  0.123890  0.164772  0.207829  0.232801  0.235254  0.213523  0.192770  0.172804  0.175454  0.174492  0.183786  0.212648  0.225156  0.246828  0.273784  0.251669  0.154232  0.179539  0.166242  0.191467  0.218061  0.207253  0.173940  0.130061  0.076130  0.044048  0.021653  0.000125  0.054861  0.064197  0.089089  0.129672  0.174544  0.207327  0.173103  0.114286 -0.021716 -0.026225  0.078511  0.194108  0.122528  0.194110  0.021277
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.011074 -0.051724 -0.521926 -0.418657 -0.474190 -0.090200 -0.109080 -0.475351 -0.043275 -0.285512 -0.983660 -0.957778 -0.941562 -0.935056 -0.942866 -0.906481 -0.875556 -0.914282 -0.731919 -0.766781 -1.070422 -1.053355 -1.089825 -1.041448 -0.961126 -0.641543 -0.685173 -0.689973 -0.515794 -0.556894 -0.690626 -0.632924 -0.460738 -0.503955 -0.629978 -0.594584 -0.539653 -0.828393 -0.923372 -0.697751 -0.803665 -0.642398 -1.068135 -1.087590 -1.057893 -1.002260 -0.933699 -0.887172 -0.914572 -0.998808 -1.029694 -1.050129 -1.053693 -1.059581 -1.072131 -1.056314 -1.042939  0.216998 -0.156305 -0.919830 -1.075647 -0.139265 -0.525147  0.009065
         1.132383 -0.763561 -0.388141 -0.740230 -0.540722  1.242703  1.253223  1.124021  1.249119  1.218183  0.614529  0.585846  0.671499  0.679013  0.624611  0.749916  0.758114  0.692177  0.936395  0.801185  0.164217  0.173133  0.057366 -0.141509 -0.219733 -0.522226 -0.438640 -0.513589 -0.627050 -0.538974 -0.308496 -0.423839 -0.699630 -0.681067 -0.433257 -0.423936 -0.609996 -0.401825 -0.255584 -0.504500 -0.394033 -0.483068  0.178324  0.132127  0.251112  0.545304  0.660449  0.734578  0.702521  0.441414  0.366467  0.284122  0.406379  0.397738  0.213787 -0.049741  0.117132  1.016057  0.878546  0.707551  0.292810  1.248877 -0.428747  1.198045
        -0.454664  0.899015  0.976031  0.750309  0.933017 -0.092764  0.024509 -0.118077 -0.069740 -0.036031  0.128118 -0.137222  0.038516 -0.003930 -0.127550  0.225437 -0.108964 -0.108570 -0.120958 -0.365552  0.351708  0.403278  0.162210  0.409426  0.591951  0.796362  0.828600  0.760199  0.807588  0.852841  0.871779  0.868160  0.769986  0.757770  0.886137  0.926096  0.809782  0.685330  0.637857  0.761964  0.725832  0.834971  0.402156  0.098934  0.348491  0.060501  0.116880  0.107397  0.013335 -0.225656 -0.192017  0.363916  0.059708  0.157871  0.364417  0.403227  0.449962 -0.610225 -0.774680  0.051427  0.274738 -0.032214  0.955059 -0.308686
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.093636  0.090629  0.108382  0.104922  0.101552  0.116313  0.129309  0.118273  0.117395  0.130654  0.143688  0.129444  0.051539  0.045600  0.047884  0.049369  0.075984  0.072544  0.073913  0.072696  0.055312  0.050803  0.071112  0.068195  0.064789  0.063350  0.006954  0.023702  0.006709  0.007234  0.029563  0.004503  0.028040  0.026246  0.001774  0.025005  0.028644  0.029196  0.031372  0.028004  0.033595  0.028500  0.034915  0.020325  0.016139  0.011001  0.015100  0.004995  0.016935  0.018277  0.006086 -0.021224 -0.028623 -0.042099 -0.039354 -0.029132 -0.020131 -0.032101 -0.034525 -0.045865 -0.040054 -0.018961 -0.017406 -0.021430 -0.027581 -0.021939 -0.024784 -0.060683 -0.064910 -0.060826 -0.074611 -0.075737 -0.061581 -0.082446 -0.064648 -0.066229 -0.074975 -0.075520 -0.077965 -0.080801 -0.074782 -0.087481 -0.085855 -0.064865 -0.063630 -0.070648 -0.066976 -0.083031 -0.070814 -0.080113 -0.072940 -0.078943 -0.074101 -0.071846 -0.090356 -0.082660 -0.066238 -0.114198 -0.120425 -0.126263 -0.106091 -0.104168 -0.111654 -0.115082 -0.105236 -0.117402 -0.107329 -0.133005 -0.131727 -0.130299 -0.129413 -0.108886 -0.133785 -0.133207 -0.133535 -0.119946 -0.119333 -0.118044 -0.107473 -0.131170 -0.106794 -0.178442 -0.188745 -0.196608 -0.200287 -0.200408 -0.178261 -0.198479 -0.177229 -0.152078 -0.150485 -0.141564 -0.196141 -0.177922 -0.177763 -0.172088 -0.216809 -0.154289 -0.166173 -0.164474 -0.155290 -0.158852 -0.209351 -0.168007 -0.167278 -0.169173 -0.147812 -0.154676 -0.139214 -0.141348 -0.141537 -0.096241 -0.140770 -0.145773 -0.139955 -0.145196 -0.141094 -0.094186 -0.099189 -0.058863 -0.100396 -0.050643 -0.047340 -0.096270 -0.103293 -0.055867 -0.042581 -0.031704 -0.014449 -0.039552 -0.013686 -0.020611 -0.027746 -0.043128 -0.031021 -0.041998 -0.037848 -0.018370 -0.044477 -0.039623 -0.041103 -0.031688 -0.013460 -0.000749 -0.019826 -0.020928 -0.012282 -0.009094  0.006445 -0.008242  0.016728  0.019602  0.005065  0.014140 -0.005381 -0.029071 -0.053536 -0.050542 -0.005208 -0.053129 -0.053024 -0.091580 -0.060104 -0.096940 -0.055056 -0.094261 -0.052550 -0.029446 -0.013095 -0.055017 -0.012341 -0.012730 -0.037736 -0.014875 -0.054215  0.035790 -0.003882  0.023064  0.040087  0.039688  0.037147  0.038634 -0.006033  0.038648 -0.005148  0.083600  0.092513  0.074761  0.093601  0.089352  0.096778  0.083176  0.033230  0.179158  0.173859  0.179981  0.165464  0.167690  0.165655  0.174833  0.143013  0.150700  0.184842  0.143757  0.158019  0.182141  0.174145  0.156745  0.152038  0.101282  0.105596  0.144604  0.131299  0.080520  0.129161  0.080936
         0.205144  0.228076  0.243182  0.261003  0.272521  0.281278  0.297241  0.306229  0.326379  0.328576  0.329792  0.320228  0.187696  0.202789  0.217147  0.231839  0.240543  0.259578  0.283280  0.295895  0.330076  0.334536  0.331735  0.335170  0.333113  0.326731  0.150069  0.167022  0.178090  0.199881  0.216756  0.229638  0.246918  0.266544  0.276238  0.302276  0.286269  0.308596  0.323331  0.354414  0.355175  0.366843  0.346764  0.355942  0.354426  0.349968  0.346448  0.340101  0.330067  0.322797  0.322070  0.318680  0.329414  0.337020  0.342963  0.346021  0.350524  0.362097  0.369960  0.381035  0.388036  0.386425  0.393737  0.402365  0.410531  0.408889  0.383343  0.285684  0.264282  0.241726  0.232462  0.196996  0.200731  0.197191  0.193781  0.189455  0.188136  0.183243  0.177052  0.167699  0.150821  0.255903  0.287602  0.310406  0.318541  0.330796  0.337868  0.352107  0.357579  0.368147  0.375279  0.379907  0.386646  0.393662  0.405147  0.411429  0.416399  0.403024  0.383983  0.376618  0.373424  0.369277  0.363804  0.356707  0.341305  0.327409  0.297541  0.272707  0.248273  0.218516  0.213388  0.193907  0.188255  0.180116  0.181617  0.183605  0.186636  0.190705  0.192440  0.202113  0.205628  0.379277  0.360497  0.305770  0.293880  0.269600  0.249475  0.219352  0.209130  0.208808  0.242782  0.260922  0.190824  0.185263  0.184410  0.190688  0.193137  0.192058  0.193724  0.182504  0.182874  0.192438  0.242045  0.292927  0.341891  0.369317  0.370123  0.383818  0.375443  0.189718  0.181627  0.173250  0.203561  0.297421  0.333047  0.360640  0.370593  0.373934  0.396308  0.422763  0.407470  0.373376  0.356736  0.334335  0.312882  0.136264  0.144476  0.136721  0.147838  0.168743  0.169138  0.180674  0.196584  0.198841  0.216177  0.222285  0.250066  0.268462  0.267893  0.286344  0.307060  0.318437  0.326158  0.345109  0.341408  0.359918  0.366353  0.393067  0.385300  0.397747  0.375339  0.379219  0.378484  0.334217  0.311357  0.338042  0.262490  0.238357  0.237524  0.203183  0.169225  0.165245  0.171805  0.185106  0.192727  0.193634  0.218774  0.355960  0.401888  0.406142  0.381048  0.370307  0.354333  0.330261  0.313017  0.327710  0.344609  0.350035  0.340911  0.341187  0.261305  0.242011  0.211896  0.212135  0.150890  0.324357  0.328222  0.331314  0.336355  0.333956  0.317094  0.298781  0.199693  0.314215  0.324536  0.303857  0.325887  0.289904  0.284897  0.282546  0.243264  0.262812  0.296519  0.279938  0.307298  0.326390  0.296929  0.330762  0.329275  0.333902  0.336121  0.313153  0.280402  0.274753  0.237158  0.207218
         0.403117  0.416023  0.423576  0.430005  0.430081  0.435390  0.433674  0.424237  0.405417  0.367079  0.377952  0.344797  0.399178  0.409224  0.415139  0.424471  0.422958  0.427918  0.425006  0.421228  0.391816  0.383549  0.392140  0.352528  0.343639  0.329243  0.398812  0.400805  0.417338  0.429204  0.424393  0.447282  0.439029  0.443910  0.470220  0.455794  0.445669  0.455025  0.455244  0.458189  0.447311  0.422390  0.395832  0.388624  0.379301  0.363623  0.356855  0.336158  0.313480  0.297771  0.288752  0.270187  0.292265  0.305502  0.322506  0.333460  0.348090  0.360885  0.372646  0.385934  0.403138  0.414595  0.427985  0.442661  0.457244  0.473531  0.529329  0.533561  0.527156  0.511221  0.514286  0.481676  0.485247  0.480243  0.464427  0.435315  0.425662  0.406216  0.388667  0.372353  0.357146  0.283019  0.278037  0.263780  0.266892  0.278178  0.294744  0.311217  0.330121  0.342213  0.358161  0.363559  0.380782  0.397321  0.406898  0.421405  0.439502  0.386688  0.348414  0.332062  0.335474  0.328877  0.315392  0.298008  0.285399  0.282427  0.281649  0.293697  0.296465  0.302733  0.306062  0.315453  0.344447  0.372652  0.394413  0.405435  0.425850  0.443618  0.453515  0.473571  0.492540  0.307937  0.303438  0.314822  0.318932  0.323032  0.316909  0.334102  0.330140  0.322221  0.305968  0.298442  0.355789  0.369960  0.392562  0.418986  0.363397  0.432430  0.430419  0.390703  0.371002  0.346922  0.333082  0.304095  0.295392  0.299254  0.304999  0.326655  0.321064  0.431094  0.368342  0.368510  0.323828  0.295023  0.287979  0.294164  0.308758  0.344261  0.383500  0.456261  0.405267  0.368238  0.342942  0.280747  0.278822  0.371074  0.384941  0.392113  0.401793  0.404990  0.417096  0.428136  0.448225  0.465517  0.471366  0.487817  0.496658  0.486143  0.510760  0.513488  0.527096  0.519014  0.497547  0.491009  0.515959  0.530807  0.517180  0.482985  0.460386  0.446792  0.458110  0.433165  0.478605  0.474660  0.486848  0.528860  0.516445  0.503060  0.458558  0.482556  0.394730  0.353661  0.392188  0.411931  0.453976  0.463257  0.495143  0.538185  0.454100  0.429229  0.408797  0.388042  0.344916  0.300424  0.259045  0.316266  0.343523  0.372049  0.403806  0.426478  0.436364  0.432280  0.445430  0.416891  0.402642  0.332604  0.347590  0.344986  0.372216  0.392459  0.411955  0.419954  0.413474  0.375483  0.393990  0.411274  0.423545  0.432412  0.432485  0.415733  0.417723  0.427724  0.404435  0.438069  0.436845  0.413992  0.424968  0.389204  0.381883  0.364080  0.383557  0.426912  0.438609  0.426972  0.418790  0.404748
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.130079  0.131696  0.125819  0.016720 -0.069108 -0.287340 -0.101434 -0.110505  0.057054  0.347531  0.323612  0.281377  0.312713  0.379384  0.347522  0.341865  0.138921  0.089974  0.116047  0.164703  0.146084  0.352351 -0.054922  0.266620  0.252487  0.236939  0.325752  0.475831  0.426379  0.485050  0.495526  0.531495  0.511810  0.629853  0.677234  0.783662  0.722658  0.850503  0.857127  0.841600  0.930134  0.778455  0.781127  0.466452  0.367356  0.209157  0.173540  0.165451  0.161085  0.197287  0.193591  0.161946  0.107555  0.158035  0.166807  0.172385  0.237561  0.277490  0.268196  0.208174  0.229173  0.264556  0.225290  0.305407  0.427538  0.631151  0.706871  0.515908  0.433199  0.432688  0.339597 -0.142054  0.251824 -0.193644 -0.021701 -0.113078 -0.092349 -0.178325 -0.320579 -0.516981 -0.766686 -0.583556 -0.540040 -0.558130 -0.353411  0.009180  0.173106  0.216857  0.257806  0.216278  0.184472  0.160617  0.200553  0.168285  0.191056  0.201351  0.244991  0.170395  0.141394  0.115696  0.136193  0.135430  0.167363  0.160734 -0.049743 -0.346552 -0.352355 -0.474412 -0.485348 -0.467276 -0.449992 -0.606158 -0.426517 -0.230748 -0.123187 -0.120121 -0.159421 -0.174056 -0.197975 -0.326137 -0.259494 -0.092198 -0.573750 -0.636803 -0.642536 -0.631899 -0.572393 -0.630558 -0.444331 -0.433230 -0.572889 -0.539246 -0.316144 -0.250927 -0.315951 -0.309795 -0.627204 -0.319355 -0.338461 -0.272143 -0.277518 -0.263379 -0.608882 -0.535770 -0.495220 -0.180768  0.048089  0.154731  0.163529 -0.267483 -0.279214 -0.435109 -0.410902 -0.492162 -0.394580 -0.116095  0.100633  0.185262  0.194705  0.242360  0.183036  0.240548  0.234875 -0.148936 -0.335317 -0.811467 -0.627501 -0.592653 -0.099200 -0.477097 -0.008023  0.022422  0.339425  0.395352  0.634441  0.605340  0.596163  0.627166  0.621870  0.665462  0.721026  0.790583  0.689037  0.737738  0.779677  0.725984  0.889851  0.717677  0.696137  0.450729  0.754522  0.626035  0.789940  0.699181  0.637701  0.742989  0.554874  0.520440  0.601871  0.436428 -0.514714 -0.694434 -0.476182 -0.102314 -0.013317 -0.142535  0.487717  0.633292  0.501288  0.242974  0.296586  0.330695  0.240446  0.147682 -0.429171  0.225027  0.180025  0.332100  0.899804  1.022788  0.502717  0.440734  0.515622  0.454110  0.126146  0.266016  0.296381  0.285201  0.135335  0.025255 -0.006824  0.037492  0.454122  0.804156  0.807463  1.092091  0.081367  0.885032  0.816101  0.963010  0.518348  0.541042  1.085052  0.253614  0.200619  1.077310  1.043865  0.320394  0.330887  0.235226  0.169681 -0.171294  0.063356  0.046544  0.285440  0.123771
        -0.284687 -0.250888 -0.198141  0.084198  0.330657  0.289156  0.736333  0.868452  1.140230  1.255372  1.265653  1.245343 -0.308088 -0.209075 -0.269576 -0.193992 -0.132131  0.191381  0.521128  0.636351  1.270563  1.202598  1.245797  1.289073  1.252444  1.221926 -0.303346 -0.283108 -0.270227 -0.259107 -0.215931 -0.141962  0.024983  0.128524  0.041535 -0.045745  0.157253 -0.033047  0.279416  0.562069  0.618727  0.956290  1.003053  1.181151  1.243098  1.253611  1.261285  1.263688  1.245743  1.233999  1.239623  1.230794  1.241885  1.247374  1.230778  1.226869  1.217259  1.173731  1.198019  1.238673  1.260257  1.238166  1.228220  1.280478  1.256324  1.162625  0.795160 -0.070808 -0.178085 -0.099567 -0.162954 -0.744300 -0.499306 -0.753832 -0.792719 -0.793625 -0.790281 -0.752033 -0.686685 -0.593316 -0.347862  0.146173  0.106005  0.666182  0.794258  1.137461  1.219562  1.187709  1.177166  1.200925  1.212359  1.230933  1.268980  1.256658  1.243055  1.221654  1.242338  1.245997  1.238098  1.214691  1.234059  1.209965  1.232862  1.158661  0.893938  0.374137  0.113291  0.147237  0.094609 -0.208024 -0.396059 -0.444479 -0.667491 -0.813040 -0.803084 -0.804506 -0.800543 -0.768340 -0.781522 -0.724818 -0.705820  1.092229  0.389743  0.144560  0.133794  0.111555  0.065564 -0.106247 -0.363837 -0.467263  0.064088  0.118499 -0.709860 -0.792875 -0.799423 -0.741957 -0.597349 -0.763202 -0.746257 -0.807776 -0.787277 -0.683145  0.014990  0.128409  0.266917  0.880083  1.206913  1.211279  1.240424 -0.776955 -0.790506 -0.721602 -0.577851  0.159916  0.287463  0.931702  1.240517  1.232476  1.242341  1.256990  1.254089  1.211276  1.174395  0.799822  0.270321 -0.013804 -0.044792  0.066054 -0.246008 -0.494885 -0.517668 -0.561571 -0.517167 -0.602049 -0.255098 -0.148648 -0.046877  0.015916 -0.060019  0.034114 -0.142823 -0.001574  0.148563  0.147388 -0.045597  0.299998  0.434256  1.012465  1.067060  1.274710  0.935289  1.157473  0.860450  0.254010  0.124782 -0.001498 -0.171758 -0.083790 -0.116848 -0.410522 -0.537480 -0.602994 -0.586905 -0.803235 -0.784728 -0.777112 -0.219328  0.210677  1.251635  1.229338  1.225239  1.191517  1.185401  1.269688  0.920730  1.248248  1.257282  1.278053  0.850647  0.521316  0.174870 -0.004042 -0.219860 -0.201954 -0.296925  1.225662  1.242024  1.261207  1.328242  1.250535  0.913986  0.671823 -0.226395  1.015307  1.096727  0.417190  1.062807  0.101578 -0.218840 -0.016652 -0.202380 -0.232091  0.281728  0.062387  0.730208  0.299261  0.194705  1.306770  1.252248  1.278938  1.329422  0.949152  0.240605  0.444742 -0.237551 -0.272617
         1.274172  1.292897  1.320755  1.410291  1.413694  1.350268  1.301981  1.218949  0.983349  0.060461  0.074980 -0.003165  1.227421  1.257137  1.235699  1.280692  1.348784  1.425438  1.395394  1.356976  0.719300  0.777931  0.777633  0.127367 -0.002192 -0.081235  1.231148  1.170845  1.206571  1.178218  1.196183  1.211607  1.285618  1.229154  1.169269  1.006541  1.153977  0.911899  1.017560  1.001274  0.803341  0.122190  0.284249  0.042212  0.084499 -0.016066 -0.014588 -0.012036 -0.061696 -0.073263 -0.061874 -0.096551 -0.093549 -0.056716 -0.088933 -0.094908 -0.078952 -0.134498 -0.099242 -0.061191 -0.005209 -0.026036 -0.066725  0.093241  0.178856  0.546715  1.072549  1.268718  1.272944  1.311198  1.334089  0.812680  1.126906  0.768672  0.622452  0.612398  0.631567  0.744875  0.830365  0.852565  0.879243 -0.578300 -0.594166 -0.530017 -0.563603 -0.307415 -0.111428 -0.149039 -0.139225 -0.125370 -0.124577 -0.103737 -0.001387 -0.047173 -0.060775 -0.091306 -0.030564 -0.069824 -0.100962 -0.156906 -0.112417 -0.156753 -0.093129 -0.231893 -0.546176 -0.689842 -0.670199 -0.637806 -0.626215 -0.536619 -0.419060 -0.272071 -0.082174  0.308398  0.568357  0.559179  0.580347  0.721278  0.661382  0.794172  0.874567 -0.387952 -0.619013 -0.568490 -0.563558 -0.565133 -0.583556 -0.506905 -0.444693 -0.358868 -0.582651 -0.607960 -0.061784  0.175243  0.506481  0.752469 -0.091253  0.677851  0.724957  0.478752  0.161558 -0.124700 -0.555074 -0.611156 -0.646326 -0.560388 -0.206279 -0.141933 -0.079130  0.653879  0.178598  0.659040 -0.238688 -0.632936 -0.677986 -0.526227 -0.121124 -0.085639 -0.061307 -0.005615 -0.045179 -0.093425 -0.158843 -0.602739 -0.692956  1.008685  1.173640  1.216942  1.317054  1.011745  1.143915  1.096882  1.044668  0.873330  1.050209  1.153671  1.210699  1.206910  1.180427  1.176236  0.992676  0.989613  1.186719  1.135635  0.975331  1.193025  0.943263  0.779888  0.704369  0.304378  0.835094  0.270196  0.868066  1.198735  1.229570  1.066109  1.180724  1.258176  1.169807  1.101991  0.930838  0.569012  0.893425  0.564094  0.660953  0.698460  1.213598  1.272809  0.338598 -0.054040 -0.029274 -0.067670 -0.136688 -0.001325 -0.441021 -0.025492 -0.021453  0.164013  0.371321  0.459147  1.317145  1.314194  1.181271  1.226780  1.287077 -0.060299 -0.003699  0.043299  0.252143  0.787141  1.207559  1.346643  1.214285  0.032192  0.346380  0.805243  1.094273  1.103198  1.000965  0.953560  1.230450  1.204065  0.839640  1.407916  1.342015 -0.030908  0.907811  0.245583  0.041100  0.065806  0.479715  1.130982  1.435504  1.411832  1.284947  1.281061
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.050015 -0.041882 -0.051337 -0.082225 -0.094444 -0.097289 -0.109555 -0.108287 -0.107502 -0.116720 -0.128000 -0.143719 -0.155230 -0.043185 -0.039286 -0.039061 -0.029852 -0.037268 -0.053943 -0.066028 -0.075000 -0.074438 -0.064468 -0.129334 -0.133234 -0.120899 -0.128861 -0.133728 -0.134789 -0.143981 -0.160478 -0.160729 -0.164712 -0.159643 -0.151512 -0.148539 -0.139808 -0.155319 -0.144760 -0.160503 -0.160182 -0.158464 -0.163278 -0.149940 -0.160947 -0.140282 -0.156297 -0.165451 -0.159060 -0.134252 -0.080468 -0.138137 -0.136645 -0.085695 -0.050305 -0.097173 -0.092815 -0.064964 -0.055899 -0.135601 -0.117901 -0.094952 -0.100887 -0.033857
        -0.008199 -0.011846 -0.053729 -0.075362 -0.052388 -0.022887 -0.056927 -0.070437 -0.086216 -0.146189 -0.145175 -0.114915 -0.105575 -0.313433 -0.325812 -0.332347 -0.348174 -0.073606 -0.098577 -0.343632 -0.330664 -0.315822 -0.301736 -0.259067 -0.272448 -0.308542 -0.324942 -0.331322 -0.189808 -0.163096 -0.162814 -0.191189 -0.145081 -0.140832 -0.200170 -0.237663 -0.279240 -0.290683 -0.322806 -0.267526 -0.252722 -0.230375 -0.208226 -0.220125 -0.288211 -0.309100 -0.210253 -0.154368 -0.154309 -0.169644 -0.114471 -0.327275 -0.274997 -0.290294 -0.300465 -0.329325 -0.106876 -0.080400 -0.320035 -0.100169 -0.133234 -0.088859 -0.041536  0.005118
        -0.285363 -0.296363 -0.328251 -0.318210 -0.275020 -0.256233 -0.264813 -0.292153 -0.318159 -0.347336 -0.308021 -0.273324 -0.228700 -0.078821 -0.112871 -0.138311 -0.177178 -0.354332 -0.379402 -0.197563 -0.151347 -0.109905 -0.079869 -0.112052 -0.132670 -0.137214 -0.177094 -0.216256 -0.308334 -0.285398 -0.229440 -0.170568 -0.191485 -0.229744 -0.137803 -0.150482 -0.148989 -0.213057 -0.225093 -0.248146 -0.207404 -0.185702 -0.205720 -0.272601 -0.241500 -0.174451 -0.161984 -0.171781 -0.241893 -0.306730 -0.377194 -0.209598 -0.140613 -0.092581 -0.067525 -0.159997 -0.364747 -0.339647 -0.105686 -0.285009 -0.329319 -0.338171 -0.260957 -0.281976
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.419407 -0.402214 -0.754075 -0.400610 -0.722557 -0.801136 -0.642197 -0.443538 -0.660710 -0.950982 -1.000835 -1.047800 -1.111351 -0.367004 -0.205825 -0.216764 -0.399321 -0.707151 -0.249948 -0.326253 -0.222836 -0.439717 -0.619969 -0.899252 -0.976077 -0.714493 -0.648627 -0.682454 -1.097828 -1.013651 -1.154209 -1.135746 -1.157974 -1.148665 -1.089749 -1.083983 -1.042289 -1.118166 -0.903656 -1.142057 -1.138801 -1.131343 -1.162370 -1.051698 -1.157344 -1.025251 -1.121890 -1.159526 -1.150392 -1.081361 -0.732104 -0.750337 -1.010577 -0.682219 -0.592775 -0.276064 -0.862818 -0.373874 -0.292883 -1.018531 -0.989435 -0.530801 -0.795727 -0.549597
         0.759941  0.763771  0.275113  0.741288  0.485884  0.277625  0.618292  0.783232  0.607488  0.046499  0.108634  0.050396  0.061416 -1.147031 -1.267743 -1.287339 -1.154459  0.520291  0.789748 -1.248609 -1.295316 -1.171789 -0.863612 -0.685775 -0.534853 -0.855151 -1.162121 -1.154154 -0.176456 -0.091267 -0.124577 -0.313908 -0.139931 -0.078914 -0.345672 -0.440491 -0.465220 -0.453511 -0.947159 -0.256496 -0.384380 -0.353517 -0.247003 -0.228826 -0.372020 -0.647298 -0.353804 -0.234852 -0.079197 -0.049400  0.173147 -1.068018 -0.477849 -0.730173 -0.891732 -1.284992  0.152996  0.766013 -1.229265  0.114216  0.353494  0.671932  0.284478  0.611406
        -0.808345 -0.814610 -0.959141 -0.799578 -0.836924 -0.899888 -0.775079 -0.690721 -0.779391 -0.863939 -0.724844 -0.667418 -0.469422  0.368678  0.178727  0.099199  0.284770 -0.799914 -0.794685  0.139038  0.066806  0.256007  0.533087  0.362444  0.337180  0.453434 -0.006876 -0.068445 -0.577362 -0.773779 -0.334492  0.014586 -0.307259 -0.363915  0.176107  0.139011  0.239518  0.002381 -0.039998 -0.439010 -0.049378  0.010112 -0.188067 -0.704755 -0.254461  0.145631  0.054867 -0.098721 -0.349691 -0.604265 -1.079082  0.067009  0.301082  0.578735  0.529241  0.073882 -0.947466 -0.773420  0.236572 -0.702708 -0.388681 -0.819038 -0.901981 -0.887308
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.071770  0.072290  0.074299  0.064672  0.069607  0.050214  0.060145  0.055426  0.020022  0.042479  0.055033  0.024764  0.029005  0.025585  0.024011  0.045176  0.030838  0.032921  0.007086  0.006340  0.031008  0.012799  0.014331  0.023461  0.026926  0.013042 -0.043885 -0.015573 -0.036972 -0.028938 -0.026963 -0.021074 -0.032146 -0.037996 -0.027501 -0.015297 -0.040501 -0.030103 -0.014481 -0.014718 -0.028688 -0.046627 -0.010960 -0.004494  0.005251  0.017807 -0.068588 -0.050726 -0.036913 -0.049767 -0.052313 -0.049138 -0.052308 -0.058956 -0.044302 -0.044739 -0.063394 -0.076898 -0.066485 -0.066110 -0.065850 -0.121060 -0.116975 -0.120153 -0.103545 -0.111019 -0.123633 -0.132040 -0.117070 -0.107935 -0.102397 -0.133645 -0.085607 -0.083125 -0.071126 -0.072001 -0.075290 -0.081914 -0.082852 -0.087420 -0.099733 -0.101934 -0.096368 -0.095673 -0.092832 -0.077403 -0.088309 -0.128804 -0.118232 -0.098338 -0.109029 -0.130885 -0.057314 -0.073816 -0.029219  0.002760 -0.003393 -0.003474  0.006041  0.004558 -0.038062  0.000297 -0.055347 -0.000319  0.057919  0.063652  0.072201  0.057806  0.050911  0.048191  0.057936  0.070325  0.070672  0.072750  0.063219  0.044244  0.040986  0.080723  0.037304  0.036954  0.036403  0.037173  0.016397  0.014069  0.048694  0.046230  0.045288  0.063314
        -0.154759 -0.173781 -0.199981 -0.213375 -0.217740 -0.244665 -0.275470 -0.282233 -0.132632 -0.165888 -0.149591 -0.154474 -0.167054 -0.213721 -0.236802 -0.227731 -0.245787 -0.267237 -0.415157 -0.414218 -0.415512 -0.403645 -0.398080 -0.395524 -0.393287 -0.391023 -0.386228 -0.390929 -0.394684 -0.396490 -0.396112 -0.398992 -0.393927 -0.344498 -0.302028 -0.276805 -0.266353 -0.224996 -0.210661 -0.176145 -0.166648 -0.171795 -0.135658 -0.135874 -0.155903 -0.145171 -0.194553 -0.185141 -0.204845 -0.257630 -0.269390 -0.325813 -0.337335 -0.375450 -0.388477 -0.388147 -0.378909 -0.371278 -0.384155 -0.381311 -0.377392 -0.346968 -0.352389 -0.344412 -0.349016 -0.320892 -0.324182 -0.297035 -0.283276 -0.276811 -0.224470 -0.325262 -0.217695 -0.225918 -0.259059 -0.280525 -0.292668 -0.345786 -0.355785 -0.362563 -0.335335 -0.311911 -0.276958 -0.267976 -0.237819 -0.370873 -0.364129 -0.318653 -0.293169 -0.360837 -0.356712 -0.338606 -0.390465 -0.375055 -0.153078 -0.176905 -0.214321 -0.260226 -0.252697 -0.288098 -0.390851 -0.409828 -0.386377 -0.389599 -0.392217 -0.391532 -0.396696 -0.396889 -0.405456 -0.415211 -0.414417 -0.405551 -0.391042 -0.399608 -0.296544 -0.412701 -0.394022 -0.391765 -0.393155 -0.401176 -0.413588 -0.417684 -0.256029 -0.162156 -0.167793 -0.270631 -0.195409 -0.146075
        -0.462527 -0.477005 -0.480704 -0.487253 -0.483578 -0.498972 -0.496811 -0.504817 -0.468756 -0.489061 -0.467688 -0.493147 -0.506602 -0.515244 -0.513233 -0.504017 -0.511957 -0.513915 -0.424752 -0.406179 -0.392479 -0.372255 -0.353075 -0.329618 -0.310931 -0.280047 -0.298549 -0.294770 -0.323054 -0.355354 -0.378563 -0.394874 -0.402417 -0.474572 -0.485840 -0.497127 -0.471732 -0.481840 -0.496390 -0.488828 -0.462504 -0.431085 -0.464842 -0.472455 -0.494060 -0.485863 -0.409956 -0.431117 -0.470494 -0.462949 -0.460224 -0.462566 -0.459510 -0.441462 -0.421216 -0.398630 -0.361469 -0.347035 -0.323914 -0.302566 -0.282563 -0.292708 -0.333395 -0.348213 -0.376762 -0.382385 -0.359400 -0.341021 -0.369054 -0.383509 -0.380611 -0.330251 -0.396966 -0.402719 -0.439338 -0.439723 -0.436184 -0.427977 -0.425645 -0.401800 -0.399777 -0.400506 -0.405657 -0.403564 -0.393679 -0.391988 -0.355585 -0.351722 -0.369393 -0.334389 -0.293745 -0.325417 -0.336216 -0.408612 -0.450442 -0.506076 -0.505367 -0.506629 -0.512047 -0.519634 -0.417934 -0.399252 -0.308189 -0.284651 -0.286647 -0.294558 -0.318275 -0.330305 -0.361754 -0.400640 -0.380146 -0.408762 -0.300671 -0.421442 -0.508778 -0.372120 -0.325999 -0.296391 -0.297129 -0.352148 -0.380204 -0.422958 -0.516410 -0.503060 -0.485174 -0.505479 -0.502664 -0.457813
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.507335  0.519672  0.666955  0.664499  0.675221  0.705054  0.626932  0.544535  0.118780  0.612832  0.442774  0.406696  0.484541  0.120309  0.068320  0.688736  0.331406  0.516575 -0.333028 -0.314680 -0.038956 -0.234614 -0.089592  0.009829  0.088335 -0.155704 -0.254233 -0.141169 -0.118321 -0.176420 -0.348549 -0.441139 -0.466683 -0.800383 -0.809923 -0.737516 -0.732481 -0.740725 -0.690154 -0.747936 -0.857976 -0.853056 -0.543035 -0.355719 -0.358809  0.337541 -0.812298 -0.891941 -0.826995 -0.767229 -0.757580 -0.760500 -0.759547 -0.675240 -0.415839 -0.456155 -0.612858 -0.643031 -0.676326 -0.417992 -0.248808 -0.813424 -0.637719 -0.811550 -0.841661 -1.005816 -0.962990 -1.088558 -0.972595 -0.970331 -0.826270 -1.117909 -0.826411 -0.876520 -0.843792 -0.818320 -0.823251 -0.885664 -0.895614 -0.901062 -0.955438 -0.962821 -0.965996 -0.970433 -0.885192 -0.652091 -0.575055 -1.054667 -0.986132 -0.526021 -0.706673 -1.039678 -0.585593 -0.565211 -0.870430 -0.497822 -0.568765 -0.620714 -0.453158 -0.514762 -0.430109 -0.370217 -0.352888 -0.126005  0.100311  0.165084  0.123299 -0.227458  0.146690  0.226829  0.322876  0.592988  0.063556  0.732371  0.418872 -0.041941  0.178320  0.201424  0.018331 -0.084714 -0.122984  0.193401 -0.090397 -0.130905  0.534291  0.630716  0.758576  0.460653
         0.535175  0.126667 -0.208616 -0.188992 -0.212707 -0.252714  0.143610  0.181482  0.558721  0.384889  0.532605  0.524392  0.519450 -0.324917 -0.246270 -0.248398 -0.244763 -0.021238 -1.351280 -1.346156 -1.390014 -1.324431 -1.370856 -1.387340 -1.388115 -1.372790 -1.313964 -1.343259 -1.370814 -1.384309 -1.342836 -1.303636 -1.294579 -0.367745 -0.226759 -0.201840 -0.307657 -0.167722 -0.222523  0.186098  0.151079  0.071076  0.599350  0.658433  0.352734  0.632216 -0.027691  0.108836 -0.016194 -0.260239 -0.289629 -0.313593 -0.347016 -0.976805 -1.316880 -1.299517 -1.206704 -1.189919 -1.176440 -1.299345 -1.341989 -1.068384 -1.199145 -0.963489 -0.961041 -0.396119 -0.526984 -0.277949 -0.185396 -0.123293 -0.154040 -0.444057 -0.010941 -0.009946 -0.186226 -0.264278 -0.282397 -0.390646 -0.464035 -0.942960 -0.571471 -0.322349 -0.139964 -0.056181 -0.054136 -1.185039 -1.235012 -0.335173 -0.221100 -1.263142 -1.157522 -0.755852 -1.223197 -1.241744  0.177202  0.221576 -0.301859 -0.169908 -0.078761  0.228951 -1.310797 -1.310527 -1.302618 -1.355173 -1.390151 -1.386370 -1.354984 -1.285905 -1.321294 -1.398929 -1.340621 -1.216994 -1.389245 -1.126912  0.049191 -1.315601 -1.324284 -1.383692 -1.392971 -1.358327 -1.370984 -1.405129 -0.032466  0.529347  0.339519  0.006944 -0.077342  0.547763
        -1.040691 -1.319371 -1.286113 -1.287011 -1.279321 -1.254697 -1.206123 -1.243560 -1.184491 -1.098439 -1.087579 -1.120258 -1.073347 -1.504518 -1.512206 -1.269134 -1.465717 -1.353894 -0.514182 -0.237547 -0.179287 -0.070692 -0.145936 -0.202677 -0.230021 -0.192529  0.009769 -0.016752 -0.121680 -0.404973 -0.361382 -0.323017 -0.404333 -1.121276 -1.104023 -1.184717 -1.192466 -1.183079 -1.233496 -1.064304 -0.922213 -0.970238 -0.885151 -0.968154 -1.274284 -1.027564 -1.057300 -0.884918 -1.053744 -1.159541 -1.168878 -1.165285 -1.166419 -0.949956 -0.426532 -0.386708 -0.474804 -0.443575 -0.304496 -0.119828 -0.092614 -0.306520 -0.442162 -0.720655 -0.660684 -0.822475 -0.863732 -0.632069 -0.877479 -0.865905 -1.066940 -0.461069 -1.036077 -0.971824 -1.069959 -1.105112 -1.099847 -1.021251 -0.998096 -0.368230 -0.860220 -0.909196 -0.879977 -0.839382 -0.975422 -0.309116 -0.423706 -0.729218 -0.860875 -0.388545 -0.332807 -0.325358 -0.501984 -0.322693 -0.878554 -1.274602 -1.325544 -1.288203 -1.383186 -1.200051 -0.417548 -0.172401 -0.039912 -0.056904 -0.334921 -0.291532 -0.037078  0.027851  0.028205 -0.420277 -0.111906 -0.670265 -0.241172 -0.610992 -1.377097  0.049003  0.014225 -0.257519 -0.299917 -0.089359 -0.139393 -0.399601 -1.485231 -1.210737 -1.197094 -1.267928 -1.193518 -1.058405
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.999129 -0.001282 0.041720]';
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.132684 -0.825432 0.548686]';
q1a = [-0.086544 -0.122370 -0.144714]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [0.230926 0.564091 0.792764]';
q1b = [-0.086544 -0.122370 -0.144714]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [-0.963882 0.021519 0.265460]';
q1c = [-0.086544 -0.122370 -0.144714]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [0.103013 -0.824382 0.556581]';
q2a = [-0.027498 0.033094 0.072403]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [0.016417 0.560891 0.827727]';
q2b = [-0.027498 0.033094 0.072403]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [-0.994545 -0.076129 0.071313]';
q2c = [-0.027498 0.033094 0.072403]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.058261 -0.827322 0.558699]';
q3a = [-0.013859 0.159919 0.261220]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [-0.459081 0.474760 0.750898]';
q3b = [-0.013859 0.159919 0.261220]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [-0.886482 -0.300235 -0.352148]';
q3c = [-0.013859 0.159919 0.261220]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [0.156397 -0.788840 0.594366]';
q4a = [-0.061749 0.209445 0.339552]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [0.880693 0.383797 0.277634]';
q4b = [-0.061749 0.209445 0.339552]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [-0.447125 0.480033 0.754750]';
q4c = [-0.061749 0.209445 0.339552]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [-0.132684 0.825432 -0.548686]';
q5a = [-0.086544 -0.122370 -0.144714]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [-0.019061 -0.555603 -0.831229]';
q5b = [-0.086544 -0.122370 -0.144714]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.990975 0.099832 -0.089454]';
q5c = [-0.086544 -0.122370 -0.144714]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.040527 0.831126 -0.554605]';
q6a = [-0.090517 -0.238172 -0.317964]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [0.679095 -0.384244 -0.625449]';
q6b = [-0.090517 -0.238172 -0.317964]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.732931 0.401977 0.548841]';
q6c = [-0.090517 -0.238172 -0.317964]';
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
