function new_config = achilles151_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.007332;
           -0.027377;
            0.355014;
            1.000000];

node2  = [ -0.007667;
            0.028652;
            0.357413;
            1.000000];

node3  = [ -0.006925;
           -0.083464;
            0.352106;
            1.000000];

node4  = [  0.069218;
           -0.017163;
            0.273749;
            1.000000];

node5  = [  0.068930;
           -0.011052;
            0.120007;
            1.000000];

node6  = [  0.013719;
           -0.001827;
           -0.098396;
            1.000000];

node7  = [  0.012823;
            0.002548;
           -0.222986;
            1.000000];

node8  = [  0.041231;
            0.011116;
           -0.292597;
            1.000000];

node9  = [ -0.136783;
           -0.000486;
           -0.374415;
            1.000000];

%%% Ordered transformation matrices
% Global rotation operator
w0a = [0.999790 -0.020328 -0.002402]';
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.009912 -0.999163 -0.039697]';
q1a = [0.068930 -0.011052 0.120007]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.244879 0.040916 -0.968690]';
q1b = [0.068930 -0.011052 0.120007]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.969503 0.000119 -0.245080]';
q1c = [0.068930 -0.011052 0.120007]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [-0.028984 -0.998972 -0.034868]';
q2a = [0.013719 -0.001827 -0.098396]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.007188 0.035090 -0.999358]';
q2b = [0.013719 -0.001827 -0.098396]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [0.999554 -0.028715 -0.008198]';
q2c = [0.013719 -0.001827 -0.098396]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.207089 -0.977666 -0.035818]';
q3a = [0.012823 0.002548 -0.222986]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [0.375414 0.113222 -0.919916]';
q3b = [0.012823 0.002548 -0.222986]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.903426 0.177058 0.390477]';
q3c = [0.012823 0.002548 -0.222986]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [0.101648 -0.991553 -0.080557]';
q4a = [0.041231 0.011116 -0.292597]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [-0.907033 -0.059114 -0.416888]';
q4b = [0.041231 0.011116 -0.292597]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [0.408605 0.115444 -0.905381]';
q4c = [0.041231 0.011116 -0.292597]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.009912 0.999163 0.039697]';
q5a = [0.068930 -0.011052 0.120007]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.001871 -0.039717 0.999209]';
q5b = [0.068930 -0.011052 0.120007]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.999949 -0.009830 -0.002264]';
q5c = [0.068930 -0.011052 0.120007]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.090612 0.995094 0.039724]';
q6a = [0.069218 -0.017163 0.273749]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [-0.682818 -0.091114 0.724884]';
q6b = [0.069218 -0.017163 0.273749]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.724947 0.038559 0.687724]';
q6c = [0.069218 -0.017163 0.273749]';
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