1function [g_moduleCM] = calculateModuleTransformations(g_joints, num_modules, module_cm)
 2
 3% Calculating the transformations from the USM base frame, F_b = F_0,
 4% to the MODULE CENTER OF MASS FRAMES, F_CM_i
 5% From the tail module and forward
 6
 7
 8g_moduleCM = zeros(4,4,num_modules);
 9
10for i = 1:num_modules
11    g_translationCM = [eye(3), module_cm(i,:)'; zeros(1,3), 1];
12    
13    if i == 1
14        g_moduleCM(:,:,i) = g_translationCM;
15    else
16        g_moduleCM(:,:,i) = g_joints(:,:,i-1) * g_translationCM;
17    end
18end 
19
20
21end
22
23
24