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 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.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.080728;
           -0.010994;
            0.148456;
            1.000000];

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

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

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

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

%%% Ordered transformation matrices
%Root Node
% Global rotation operator
w1a = [0.999790 -0.020328 -0.002402]';
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.920157 0.011244 -0.391388]';
q1c = [-0.007332 -0.027377 0.355014]';
t1c = twist_matrix_fast(w1c,q1c,Theta(1));

w1d = [0.391128 0.072769 -0.917455]';
q1d = [-0.007332 -0.027377 0.355014]';
t1d = twist_matrix_fast(w1d,q1d,Theta(2));

w1e = [0.018165 -0.997285 -0.071356]';
q1e = [-0.007332 -0.027377 0.355014]';
t1e = twist_matrix_fast(w1e,q1e,Theta(3));

w4a = [0.060252 -0.996755 -0.053372]';
q4a = [0.080728 -0.010994 0.148456]';
t4a = twist_matrix_fast(w4a,q4a,Theta(4));

w4b = [-0.261804 0.035817 -0.964456]';
q4b = [0.080728 -0.010994 0.148456]';
t4b = twist_matrix_fast(w4b,q4b,Theta(5));

w4c = [-0.963239 -0.072083 0.258796]';
q4c = [0.080728 -0.010994 0.148456]';
t4c = twist_matrix_fast(w4c,q4c,Theta(6));

w5a = [-0.007657 -0.999357 -0.035035]';
q5a = [0.013719 -0.001827 -0.098396]';
t5a = twist_matrix_fast(w5a,q5a,Theta(7));

w5b = [-0.007188 0.035090 -0.999358]';
q5b = [0.013719 -0.001827 -0.098396]';
t5b = twist_matrix_fast(w5b,q5b,Theta(8));

w5c = [-0.999945 0.007400 0.007452]';
q5c = [0.013719 -0.001827 -0.098396]';
t5c = twist_matrix_fast(w5c,q5c,Theta(9));

w6a = [0.207089 -0.977666 -0.035818]';
q6a = [0.012823 0.002548 -0.222986]';
t6a = twist_matrix_fast(w6a,q6a,Theta(10));

w6b = [0.375414 0.113222 -0.919916]';
q6b = [0.012823 0.002548 -0.222986]';
t6b = twist_matrix_fast(w6b,q6b,Theta(11));

w6c = [-0.903426 -0.177058 -0.390477]';
q6c = [0.012823 0.002548 -0.222986]';
t6c = twist_matrix_fast(w6c,q6c,Theta(12));

w7a = [0.101648 -0.991553 -0.080557]';
q7a = [0.041231 0.011116 -0.292597]';
t7a = twist_matrix_fast(w7a,q7a,Theta(13));

w7b = [-0.907033 -0.059114 -0.416888]';
q7b = [0.041231 0.011116 -0.292597]';
t7b = twist_matrix_fast(w7b,q7b,Theta(14));

w7c = [-0.408605 -0.115444 0.905381]';
q7c = [0.041231 0.011116 -0.292597]';
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
