1function [g_joints, g_joints_home] = calculateJointTransformations(q, num_joints, joint_tr, joint_ax)
2
3
4
5
6
7
8
9
10g_joints = zeros(4,4,num_joints);
11g_joints_home = zeros(4,4,num_joints+1);
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