function new_config = gatsby140_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.015708;
            0.048890;
           -0.312476;
            1.000000];

node2  = [  0.017453;
            0.074706;
           -0.307998;
            1.000000];

node3  = [  0.013963;
            0.021353;
           -0.317258;
            1.000000];

node4  = [ -0.049932;
            0.025219;
           -0.169989;
            1.000000];

node5  = [  0.002680;
           -0.002102;
           -0.002603;
            1.000000];

node6  = [  0.006943;
           -0.017204;
            0.123346;
            1.000000];

node7  = [ -0.027136;
           -0.025934;
            0.175473;
            1.000000];

node8  = [  0.107975;
           -0.037209;
            0.259011;
            1.000000];

%%% Ordered transformation matrices
%Root Node
% Global rotation operator
w1a = [-0.999166 0.040392 0.005912]';
q1a = [0.0 0.0 0.0]';
t1b = twist_matrix_fast(w1a,q1a,sp_coords(4));

% Global translation operator
t1a = twist_matrix_fast(sp_coords(1:3),0,0);

w1c = [0.908565 -0.130276 0.396909]';
q1c = [0.015708 0.048890 -0.312476]';
t1c = twist_matrix_fast(w1c,q1c,Theta(1));

w1d = [-0.413727 -0.149197 0.898092]';
q1d = [0.015708 0.048890 -0.312476]';
t1d = twist_matrix_fast(w1d,q1d,Theta(2));

w1e = [-0.057782 -0.980188 -0.189454]';
q1e = [0.015708 0.048890 -0.312476]';
t1e = twist_matrix_fast(w1e,q1e,Theta(3));

w4a = [0.003701 -0.986748 -0.162220]';
q4a = [-0.049932 0.025219 -0.169989]';
t4a = twist_matrix_fast(w4a,q4a,Theta(4));

w4b = [0.296280 -0.153856 0.942627]';
q4b = [-0.049932 0.025219 -0.169989]';
t4b = twist_matrix_fast(w4b,q4b,Theta(5));

w4c = [0.955094 0.051551 -0.291784]';
q4c = [-0.049932 0.025219 -0.169989]';
t4c = twist_matrix_fast(w4c,q4c,Theta(6));

w5a = [-0.151657 -0.981997 -0.112614]';
q5a = [0.002680 -0.002102 -0.002603]';
t5a = twist_matrix_fast(w5a,q5a,Theta(7));

w5b = [0.033589 -0.118986 0.992328]';
q5b = [0.002680 -0.002102 -0.002603]';
t5b = twist_matrix_fast(w5b,q5b,Theta(8));

w5c = [0.987862 -0.146710 -0.051029]';
q5c = [0.002680 -0.002102 -0.002603]';
t5c = twist_matrix_fast(w5c,q5c,Theta(9));

w6a = [0.068498 -0.990279 -0.121059]';
q6a = [0.006943 -0.017204 0.123346]';
t6a = twist_matrix_fast(w6a,q6a,Theta(10));

w6b = [-0.541912 -0.138815 0.828892]';
q6b = [0.006943 -0.017204 0.123346]';
t6b = twist_matrix_fast(w6b,q6b,Theta(11));

w6c = [0.837639 -0.008826 0.546153]';
q6c = [0.006943 -0.017204 0.123346]';
t6c = twist_matrix_fast(w6c,q6c,Theta(12));

w7a = [0.014132 -0.987631 -0.156159]';
q7a = [-0.027136 -0.025934 0.175473]';
t7a = twist_matrix_fast(w7a,q7a,Theta(13));

w7b = [0.848419 -0.070802 0.524569]';
q7b = [-0.027136 -0.025934 0.175473]';
t7b = twist_matrix_fast(w7b,q7b,Theta(14));

w7c = [0.529136 0.139902 -0.836924]';
q7c = [-0.027136 -0.025934 0.175473]';
t7c = twist_matrix_fast(w7c,q7c,Theta(15));

%%% Construct kinematic chains of transformations
U1  = t1a*t1b*node1;
U2  = t1a*t1b*node2;
U3  = t1a*t1b*node3;
U4  = t1a*t1b*t1c*t1d*t1e*t4a*t4b*t4c*node4;
U5  = t1a*t1b*t1c*t1d*t1e*t4a*t4b*t4c*t5a*t5b*t5c*node5;
U6  = t1a*t1b*t1c*t1d*t1e*t4a*t4b*t4c*t5a*t5b*t5c*t6a*t6b*t6c*node6;
U7  = t1a*t1b*t1c*t1d*t1e*t4a*t4b*t4c*t5a*t5b*t5c*t6a*t6b*t6c*t7a*t7b*t7c*node7;
U8  = t1a*t1b*t1c*t1d*t1e*t4a*t4b*t4c*t5a*t5b*t5c*t6a*t6b*t6c*t7a*t7b*t7c*node8;

%%% 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,:)];

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