 1function [g_joints, g_joints_home] = calculateJointTransformations(q, num_joints, joint_tr, joint_ax)
 2
 3% Calculates the transformation matrices from 
 4% the USM base frame, F_b = F_0, to the joint frames F_i (i=1..n)
 5% From the tail module and forward
 6
 7
 8
 9% JOINT FRAMES
10g_joints = zeros(4,4,num_joints);  
11g_joints_home = zeros(4,4,num_joints+1);  % size = num_joint+1 to include F_e
12
13for i = 1:num_joints
14    if strcmp(joint_ax(i),'Y')
15        g_nextJoint = [rotationY(q(i)), joint_tr(i,:)'; zeros(1,3), 1];
16        g_nextJoint_home = [eye(3), joint_tr(i,:)'; zeros(1,3), 1];
17    elseif strcmp(joint_ax(i),'Z')
18        g_nextJoint = [rotationZ(q(i)), joint_tr(i,:)'; zeros(1,3), 1];
19        g_nextJoint_home = [eye(3), joint_tr(i,:)'; zeros(1,3), 1];
20    else
21        g_nextJoint = eye(4);
22        g_nextJoint_home = eye(4);
23    end
24    
25    if i == 1
26        g_joints(:,:,i) = g_nextJoint;
27        g_joints_home(:,:,i) = g_nextJoint_home;
28    else
29        g_joints(:,:,i) = g_joints(:,:,i-1) * g_nextJoint;
30        g_joints_home(:,:,i) = g_joints(:,:,i-1) * g_nextJoint_home;       
31    end
32end
33
34g_ne = [eye(3), joint_tr(end,:)'; zeros(1,3), 1];
35g_joints_home(:,:,end) = g_joints(:,:,num_joints) * g_ne;
36
37
38end
39
40
41