1function [ g_If,R_If,eta_f,g_Ib,R_Ib,eta_b,q,g_joints_home,g_moduleCM,g_thrusters ] = calculateTransformations( q1, q2, eta_marker, g_marker2front, controlCenterLink )
2
3
4modelParams = coder.load('eely_params.mat');
5
6
7num_joints = modelParams.num_joints;
8num_modules = modelParams.num_modules;
9num_thrusters = modelParams.num_thrusters;
10
11module_cm = modelParams.module_cm;
12joint_tr = modelParams.joint_tr;
13joint_ax = modelParams.joint_ax;
14
15thruster_ax = modelParams.thruster_ax;
16thruster_pos = modelParams.thruster_pos;
17thruster_moduleNumber = modelParams.thruster_moduleNumber;
18
19
20
21
22
23R_marker = Rzyx(eta_marker(4),eta_marker(5),eta_marker(6));
24g_marker = [R_marker, eta_marker(1:3); zeros(1,3), 1];
25g_If = g_marker * g_marker2front;
26
27p_If_I = g_If(1:3,4);
28R_If = g_If(1:3,1:3);
29eta_f = [p_If_I; rot2euler(R_If)];
30
31
32
33
34q = [q2(4),q1(4),q2(3),q1(3),q2(2),q1(2),q2(5),q1(5)]';
35q = q * pi/180;
36
37
38
39
40
41
42[g_joints, g_joints_home] = calculateJointTransformations(q, num_joints, joint_tr, joint_ax);
43
44
45[g_moduleCM] = calculateModuleTransformations(g_joints, num_modules, module_cm);
46
47
48[g_thrusters] = calculateThrusterTransformations(g_joints, num_thrusters, thruster_ax, thruster_pos, thruster_moduleNumber);
49
50
51g_front2back = calculateTransformationInverse(g_joints_home(1:4,1:4,num_joints+1));
52
53
54
55if controlCenterLink
56
57
58 g_back2center = g_moduleCM(1:4,1:4,5);
59 g_center2back = calculateTransformationInverse(g_back2center);
60
61 g_Ib = g_If * g_front2back * g_back2center;
62 for i = 1:num_thrusters
63 g_thrusters(1:4,1:4,i) = g_center2back * g_thrusters(1:4,1:4,i);
64 end
65 for i = 1:num_modules
66 g_moduleCM(1:4,1:4,i) = g_center2back * g_moduleCM(1:4,1:4,i);
67 end
68else
69 g_Ib = g_If * g_front2back;
70end
71
72p_Ib_I = g_Ib(1:3,4);
73R_Ib = g_Ib(1:3,1:3);
74eta_b = [p_Ib_I; rot2euler(R_Ib)];
75
76end
77