Add HAC-LAC Tracking control simulations

src/computePosErrorTest.m

@@ -0,0 +1,50 @@
+function [Edx, Edy, Edz, Erx, Ery, Erz] = computePosErrorTest(Dxr, Dyr, Dzr, Rxr, Ryr, Rzr, Dxm, Dym, Dzm, Rxm, Rym, Rzm)
+
+%% Measured Pose
+WTm = zeros(4,4);
+
+WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0;
+     sin(Rzm)  cos(Rzm) 0;
+     0        0         1] * ...
+    [cos(Rym)  0        sin(Rym);
+     0        1        0;
+     -sin(Rym)  0        cos(Rym)] * ...
+    [1        0        0;
+     0        cos(Rxm) -sin(Rxm);
+     0        sin(Rxm)  cos(Rxm)];
+WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1];
+
+%% Reference Pose
+WTr = zeros(4,4);
+
+WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0;
+     sin(Rzr)  cos(Rzr) 0;
+     0        0         1] * ...
+    [cos(Ryr)  0        sin(Ryr);
+     0        1        0;
+     -sin(Ryr)  0        cos(Ryr)] * ...
+    [1        0        0;
+     0        cos(Rxr) -sin(Rxr);
+     0        sin(Rxr)  cos(Rxr)];
+WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1];
+
+WTv = eye(4);
+WTv(1:3, 4) = WTm(1:3, 4);
+
+%% Wanted pose expressed in a frame corresponding to the actual measured pose
+MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
+
+%% Wanted pose expressed in a frame coincident with the actual position but with no rotation
+VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr;
+
+%% Extract Translations and Rotations from the Homogeneous Matrix
+  T = MTr;
+  Edx = T(1, 4);
+  Edy = T(2, 4);
+  Edz = T(3, 4);
+
+  % The angles obtained are u-v-w Euler angles (rotations in the moving frame)
+  Ery = atan2( T(1, 3),          sqrt(T(1, 1)^2 + T(1, 2)^2));
+  Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
+  Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
+end

src/initializeController.m

@@ -7,7 +7,7 @@ function [controller] = initializeController(args)
 %    - args - Can have the following fields:
 
 arguments
-  args.type   char   {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X'})} = 'open-loop'
+  args.type   char   {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop'
 end
 
 controller = struct();
@@ -27,4 +27,6 @@ switch args.type
     controller.type = 5;
   case 'ref-track-X'
     controller.type = 6;
+  case 'ref-track-hac-dvf'
+    controller.type = 7;
 end