function new_config = cicero_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 N x 1 angles, where N is the number of rotation axes
%%% Outputs %%%
% new_config: a sparse point cloud representing the nodes of kinematic model in the new configuration.
node1  = [  0.166766;
           -0.095816;
            0.408682;
            1.000000];

node2  = [  0.167997;
           -0.144208;
            0.395887;
            1.000000];

node3  = [  0.166977;
           -0.042376;
            0.422755;
            1.000000];

node4  = [  0.012602;
           -0.073168;
            0.301482;
            1.000000];

node5  = [ -0.039071;
           -0.021976;
            0.130688;
            1.000000];

node6  = [ -0.073871;
            0.034944;
           -0.071569;
            1.000000];

node7  = [ -0.132058;
            0.071684;
           -0.233441;
            1.000000];

node8  = [ -0.182555;
            0.087679;
           -0.300782;
            1.000000];

node9  = [ -0.025075;
            0.137495;
           -0.467578;
            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
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
