function new_config = cicero_norm_km_frame(sp_coords,Theta)

% This code represents the skeleton of the kinematic model created by 'Build_data_frame' as a function.

%%% INPUTS %%%
% sp_coords: a 4 x 1 vector for global position, R3, and rotation, [-pi,pi], in the spatial frame.
% Theta: a vector of 18 x 1 angles about joint rotation axes.
%%% Outputs %%%
% new_config: a sparse point cloud representing the nodes of kinematic model in the new configuration.
node1  = [  0.012052;
           -0.451457;
            0.007376;
            1.000000];

node2  = [ -0.037998;
           -0.451957;
            0.006044;
            1.000000];

node3  = [  0.067314;
           -0.451457;
            0.007376;
            1.000000];

node4  = [  0.006066;
           -0.291182;
            0.107612;
            1.000000];

node5  = [  0.011879;
           -0.106801;
            0.086840;
            1.000000];

node6  = [  0.015285;
            0.100525;
            0.038227;
            1.000000];

node7  = [  0.009370;
            0.276029;
            0.028158;
            1.000000];

node8  = [  0.007496;
            0.359314;
            0.048173;
            1.000000];

node9  = [  0.013796;
            0.459407;
           -0.164063;
            1.000000];

%%% Ordered transformation matrices
% Global rotation operator
w0a = [-0.154364 0.000000 0.988014]';
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.991391 -0.045106 -0.122924]';
q1a = [0.011879 -0.106801 0.086840]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [0.015989 0.973469 -0.228258]';
q1b = [0.011879 -0.106801 0.086840]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.129958 0.224327 0.965810]';
q1c = [0.011879 -0.106801 0.086840]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [0.961100 0.047993 0.271999]';
q2a = [0.015285 0.100525 0.038227]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.033625 0.997794 -0.057242]';
q2b = [0.015285 0.100525 0.038227]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [-0.274146 0.045870 0.960593]';
q2c = [0.015285 0.100525 0.038227]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.998797 0.031504 -0.037565]';
q3a = [0.009370 0.276029 0.028158]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [-0.021875 0.972085 0.233607]';
q3b = [0.009370 0.276029 0.028158]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.043875 -0.232505 0.971605]';
q3c = [0.009370 0.276029 0.028158]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [0.999251 0.013795 0.036167]';
q4a = [0.007496 0.359314 0.048173]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [0.026838 0.426400 -0.904136]';
q4b = [0.007496 0.359314 0.048173]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [-0.027894 0.904429 0.425710]';
q4c = [0.007496 0.359314 0.048173]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [-0.991391 0.045106 0.122924]';
q5a = [0.011879 -0.106801 0.086840]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [-0.031316 -0.993226 0.111895]';
q5b = [0.011879 -0.106801 0.086840]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.127138 0.107082 0.986088]';
q5c = [0.011879 -0.106801 0.086840]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.995456 0.020920 -0.092899]';
q6a = [0.006066 -0.291182 0.107612]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [0.031650 -0.847421 -0.529977]';
q6b = [0.006066 -0.291182 0.107612]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [-0.089812 -0.530509 0.842908]';
q6c = [0.006066 -0.291182 0.107612]';
t6c = twist_matrix_fast(w6c,q6c,Theta(18));

%%% Construct kinematic chains of transformations
U5  = t0a*t0b*node5;
U6  = t0a*t0b*t1a*t1b*t1c*node6;
U7  = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*node7;
U8  = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*node8;
U9  = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*t4a*t4b*t4c*node9;
U4  = t0a*t0b*t5a*t5b*t5c*node4;
U1  = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*node1;
U2  = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*node2;
U3  = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*node3;

%%% Construct complete update
new_config = [U1(1:3,:) U2(1:3,:) U3(1:3,:) U4(1:3,:) U5(1:3,:) U6(1:3,:) U7(1:3,:) U8(1:3,:) U9(1:3,:)];

%%% 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
