function new_config = buddy140_km_frame(ctrm,sp_coords,Theta)

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

%%% INPUTS %%%
% ctrm: the estimated center of mass for the kinematic model configuration.
% sp_coords: a 4 x 1 vector for global rotation, [-pi,pi], and position, R3, 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.007455
        -0.193970
         0.228362
         1.000000];

node2  = [  0.005877
        -0.161633
         0.255141
         1.000000];

node3  = [  0.007455
        -0.227452
         0.202203
         1.000000];

node4  = [  0.028378
        -0.095224
         0.102339
         1.000000];

node5  = [ -0.004649
        -0.001896
        -0.014092
         1.000000];

node6  = [ -0.006658
         0.073573
        -0.101173
         1.000000];

node7  = [  0.002936
         0.094724
        -0.121438
         1.000000];

node8  = [ -0.130241
         0.155619
        -0.167032
         1.000000];

%%% Ordered transformation matrices
%Root Node
% Global rotation operator
w1a = [0.994421 0.094730 -0.046401]';
q1a = [ctrm(1) ctrm(2) ctrm(3)]';
t1a = twist_matrix(w1a,q1a,sp_coords(1));

% Global translation operator
t1b = t1a\twist_matrix(sp_coords(2:4),0,0)*t1a;

w1c = [0.206218 -0.011096 0.025543]';
q1c = [0.007455 -0.193970 0.228362]';
t1c = twist_matrix(w1c,q1c,Theta(1));

w1d = [-0.007811 0.160154 0.132631]';
q1d = [0.007455 -0.193970 0.228362]';
t1d = twist_matrix(w1d,q1d,Theta(2));

w1e = [-0.026731 -0.132397 0.158297]';
q1e = [0.007455 -0.193970 0.228362]';
t1e = twist_matrix(w1e,q1e,Theta(3));

w4a = [0.006539 0.163188 0.128952]';
q4a = [0.028378 -0.095224 0.102339]';
t4a = twist_matrix(w4a,q4a,Theta(4));

w4b = [0.026965 0.127261 -0.162416]';
q4b = [0.028378 -0.095224 0.102339]';
t4b = twist_matrix(w4b,q4b,Theta(5));

w4c = [-0.206232 0.021814 -0.017147]';
q4c = [0.028378 -0.095224 0.102339]';
t4c = twist_matrix(w4c,q4c,Theta(6));

w5a = [-0.038489 0.154098 0.134439]';
q5a = [-0.004649 -0.001896 -0.014092]';
t5a = twist_matrix(w5a,q5a,Theta(7));

w5b = [0.003627 -0.136263 0.157228]';
q5b = [-0.004649 -0.001896 -0.014092]';
t5b = twist_matrix(w5b,q5b,Theta(8));

w5c = [0.204467 0.031425 0.022517]';
q5c = [-0.004649 -0.001896 -0.014092]';
t5c = twist_matrix(w5c,q5c,Theta(9));

w6a = [-0.053929 0.151262 0.132337]';
q6a = [-0.006658 0.073573 -0.101173]';
t6a = twist_matrix(w6a,q6a,Theta(10));

w6b = [0.064774 0.142788 -0.136811]';
q6b = [-0.006658 0.073573 -0.101173]';
t6b = twist_matrix(w6b,q6b,Theta(11));

w6c = [-0.190256 0.005737 -0.084090]';
q6c = [-0.006658 0.073573 -0.101173]';
t6c = twist_matrix(w6c,q6c,Theta(12));

w7a = [0.012111 0.140829 0.152715]';
q7a = [0.002936 0.094724 -0.121438]';
t7a = twist_matrix(w7a,q7a,Theta(13));

w7b = [0.180690 -0.082620 0.061859]';
q7b = [0.002936 0.094724 -0.121438]';
t7b = twist_matrix(w7b,q7b,Theta(14));

w7c = [0.102499 0.129006 -0.127094]';
q7c = [0.002936 0.094724 -0.121438]';
t7c = twist_matrix(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(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.
%
% [twist] = twist_matrix(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)
    % 1) pure translation
    % In this case the input w now is a translation vector
	% check input
	if numel(w) < 3
		error('Input error: w must be a 3d vector.')
	end
	% we want column vectors
	if size(w,1)<size(w,2)
		w = w';
	end
	% pure translation in absolute frame
	twist = [eye(3) w;zeros(1,3) 1];
else
    % 2) pure rotation twist
    if numel(w)<3 || numel(q) < 3
        error('Input error: Both w and q must be 3d.')
    end
    if size(w) ~= size(q)
        error('Input error: w and q must be the same size.')
    end
    % we want column vectors
    if size(w,1)<size(w,2)
        w = w';
        q = q';
    end
    % correct for case where w is the origin
    if sum(w) == 0
        w = q;
    end
    
    % normalize
    if norm(w) ~= 1 && sum(w)~=0
        % make unit vector
        w = w/norm(w);
    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
    w_hat2 = w*w'-norm(w)^2*eye(3);
    R      = eye(3)+w_hat*sin(theta)+w_hat2*(1-cos(theta));

    % Generate relative frame twist, pg. 42
    twist  = [R (eye(3)-R)*w_hat*v+w*w'*v*theta;
              zeros(1,3) 1];
end
