diff --git a/scaled_teleoperation.mdl b/scaled_teleoperation.mdl
index 9f6c24c06d474ce415a872552e3e467494973b73..7f5383a5036f47be08d40314bfd95de93a5667e2 100644
--- a/scaled_teleoperation.mdl
+++ b/scaled_teleoperation.mdl
@@ -384,7 +384,7 @@ Model {
       Name		      "ext2irb.robot[0].mocgendata.string2[i]"
     }
     ParameterArgumentNames  ""
-    ComputedModelVersion    "1.2866"
+    ComputedModelVersion    "1.2887"
     NumModelReferences	    0
     NumTestPointedSignals   21
     TestPointedSignal {
@@ -496,9 +496,9 @@ Model {
   ModifiedByFormat	  "%<Auto>"
   LastModifiedBy	  "martinka"
   ModifiedDateFormat	  "%<Auto>"
-  LastModifiedDate	  "Tue Sep 17 20:31:39 2019"
-  RTWModifiedTimeStamp	  490651023
-  ModelVersionFormat	  "1.%<AutoIncrement:2866>"
+  LastModifiedDate	  "Thu Sep 26 17:10:10 2019"
+  RTWModifiedTimeStamp	  491418270
+  ModelVersionFormat	  "1.%<AutoIncrement:2887>"
   ConfigurationManager	  "none"
   SampleTimeColors	  off
   SampleTimeAnnotations	  off
@@ -566,9 +566,9 @@ Model {
   ExtModeSkipDownloadWhenConnect off
   ExtModeLogAll		  on
   ExtModeAutoUpdateStatusClock off
-  TunableVars		  "a_switch,f_switch,highKiActive,keepStill,leftActive,rightActive"
-  TunableVarsStorageClass "Auto,Auto,Auto,Auto,Auto,Auto"
-  TunableVarsTypeQualifier ",,,,,"
+  TunableVars		  "a_switch,f_switch,highKiActive,keepStill,leftActive,rightActive,scaleFactor"
+  TunableVarsStorageClass "Auto,Auto,Auto,Auto,Auto,Auto,Auto"
+  TunableVarsTypeQualifier ",,,,,,"
   BufferReuse		  on
   ShowModelReferenceBlockVersion off
   ShowModelReferenceBlockIO off
@@ -1103,8 +1103,8 @@ Model {
       }
       Name		      "Configuration"
       ExtraOptions	      "-aGenerateTraceInfo=0 -aIgnoreTestpoints=0 "
-      CurrentDlgPage	      "Code Generation/RTW2ORCA"
-      ConfigPrmDlgPosition     [ 427, 27, 1493, 1150 ] 
+      CurrentDlgPage	      "Optimization/Signals and Parameters"
+      ConfigPrmDlgPosition     [ 427, 64, 1493, 1187 ] 
     }
     PropName		    "ConfigurationSets"
   }
@@ -1345,7 +1345,7 @@ Model {
     ShowPageBoundaries	    off
     ZoomFactor		    "100"
     ReportName		    "simulink-default.rpt"
-    SIDHighWatermark	    "1421"
+    SIDHighWatermark	    "1422"
     Block {
       BlockType		      Inport
       Name		      "irb2ext.obtaining"
@@ -5189,7 +5189,7 @@ Model {
       MaskHideContents	      off
       System {
 	Name			"Right"
-	Location		[0, -28, 1920, 1080]
+	Location		[0, 46, 1920, 1154]
 	Open			off
 	ModelBrowserVisibility	off
 	ModelBrowserWidth	200
@@ -7907,7 +7907,7 @@ Model {
       System {
 	Name			"botharms_get_torque"
 	Location		[0, 18, 1920, 1126]
-	Open			on
+	Open			off
 	ModelBrowserVisibility	off
 	ModelBrowserWidth	200
 	ScreenColor		"white"
@@ -7919,7 +7919,7 @@ Model {
 	TiledPageScale		1
 	ShowPageBoundaries	off
 	ZoomFactor		"100"
-	SIDHighWatermark	"497"
+	SIDHighWatermark	"498"
 	SIDPrevWatermark	"325"
 	Block {
 	  BlockType		  Inport
@@ -8016,6 +8016,15 @@ Model {
 	  ZOrder		  -5
 	  Value			  "zeros(7,1)"
 	}
+	Block {
+	  BlockType		  Constant
+	  Name			  "Constant3"
+	  SID			  "1345:498"
+	  Position		  [600, 1029, 715, 1051]
+	  ShowName		  off
+	  Value			  "scaleFactor"
+	  VectorParams1D	  off
+	}
 	Block {
 	  BlockType		  Demux
 	  Name			  "Demux"
@@ -9574,8 +9583,8 @@ Model {
 	  BlockType		  SubSystem
 	  Name			  "MATLAB Function1"
 	  SID			  "1345:350"
-	  Ports			  [2, 2]
-	  Position		  [740, 818, 790, 922]
+	  Ports			  [3, 1]
+	  Position		  [750, 818, 800, 922]
 	  ZOrder		  -4
 	  LibraryVersion	  "1.32"
 	  ErrorFcn		  "Stateflow.Translate.translate"
@@ -9622,7 +9631,7 @@ Model {
 	    TiledPageScale	    1
 	    ShowPageBoundaries	    off
 	    ZoomFactor		    "100"
-	    SIDHighWatermark	    "27"
+	    SIDHighWatermark	    "29"
 	    Block {
 	      BlockType		      Inport
 	      Name		      "T44_FK1"
@@ -9640,6 +9649,15 @@ Model {
 	      Port		      "2"
 	      IconDisplay	      "Port number"
 	    }
+	    Block {
+	      BlockType		      Inport
+	      Name		      "scale"
+	      SID		      "1345:350::28"
+	      Position		      [20, 171, 40, 189]
+	      ZOrder		      14
+	      Port		      "3"
+	      IconDisplay	      "Port number"
+	    }
 	    Block {
 	      BlockType		      Demux
 	      Name		      " Demux "
@@ -9654,11 +9672,11 @@ Model {
 	      Name		      " SFunction "
 	      SID		      "1345:350::19"
 	      Tag		      "Stateflow S-Function scaled_teleoperation 11"
-	      Ports		      [2, 3]
+	      Ports		      [3, 2]
 	      Position		      [180, 104, 230, 206]
 	      ZOrder		      5
 	      FunctionName	      "sf_sfun"
-	      PortCounts	      "[2 3]"
+	      PortCounts	      "[3 2]"
 	      EnableBusSupport	      on
 	      Port {
 		PortNumber		2
@@ -9666,12 +9684,6 @@ Model {
 		RTWStorageClass		"Auto"
 		DataLoggingNameMode	"SignalName"
 	      }
-	      Port {
-		PortNumber		3
-		Name			"scale"
-		RTWStorageClass		"Auto"
-		DataLoggingNameMode	"SignalName"
-	      }
 	    }
 	    Block {
 	      BlockType		      Terminator
@@ -9688,15 +9700,6 @@ Model {
 	      ZOrder		      11
 	      IconDisplay	      "Port number"
 	    }
-	    Block {
-	      BlockType		      Outport
-	      Name		      "scale"
-	      SID		      "1345:350::27"
-	      Position		      [460, 136, 480, 154]
-	      ZOrder		      13
-	      Port		      "2"
-	      IconDisplay	      "Port number"
-	    }
 	    Line {
 	      SrcBlock		      "T44_FK1"
 	      SrcPort		      1
@@ -9709,6 +9712,12 @@ Model {
 	      DstBlock		      " SFunction "
 	      DstPort		      2
 	    }
+	    Line {
+	      SrcBlock		      "scale"
+	      SrcPort		      1
+	      DstBlock		      " SFunction "
+	      DstPort		      3
+	    }
 	    Line {
 	      Name		      "e"
 	      Labels		      [0, 0]
@@ -9717,14 +9726,6 @@ Model {
 	      DstBlock		      "e"
 	      DstPort		      1
 	    }
-	    Line {
-	      Name		      "scale"
-	      Labels		      [0, 0]
-	      SrcBlock		      " SFunction "
-	      SrcPort		      3
-	      DstBlock		      "scale"
-	      DstPort		      1
-	    }
 	    Line {
 	      SrcBlock		      " Demux "
 	      SrcPort		      1
@@ -12444,7 +12445,7 @@ Model {
 	  Branch {
 	    Points		    [40, 0]
 	    Branch {
-	      Points		      [195, 0]
+	      Points		      [205, 0]
 	      DstBlock		      "MATLAB Function1"
 	      DstPort		      1
 	    }
@@ -12544,7 +12545,7 @@ Model {
 	  SrcPort		  1
 	  Points		  [10, 0]
 	  Branch {
-	    Points		    [195, 0; 0, 275]
+	    Points		    [195, 0; 0, 250]
 	    DstBlock		    "MATLAB Function1"
 	    DstPort		    2
 	  }
@@ -12673,7 +12674,7 @@ Model {
 	  Labels		  [0, 0]
 	  SrcBlock		  "MATLAB Function1"
 	  SrcPort		  1
-	  Points		  [20, 0]
+	  Points		  [10, 0]
 	  DstBlock		  "haptic_controller"
 	  DstPort		  11
 	}
@@ -13049,6 +13050,21 @@ Model {
 	  DstBlock		  "Terminator11"
 	  DstPort		  1
 	}
+	Line {
+	  SrcBlock		  "Constant3"
+	  SrcPort		  1
+	  Points		  [20, 0; 0, -20]
+	  Branch {
+	    Points		    [0, -115]
+	    DstBlock		    "MATLAB Function1"
+	    DstPort		    3
+	  }
+	  Branch {
+	    Points		    [75, 0]
+	    DstBlock		    "haptic_controller"
+	    DstPort		    12
+	  }
+	}
       }
     }
     Block {
@@ -14015,18 +14031,18 @@ Model {
       Name		      "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/robot/m"
       "atlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/bp-ar"
       "-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkeepSti"
-      "ll = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDithering=15"
-      ";\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_switch"
-      " = 0;\ndisp('Init done');"
+      "ll = 0;\nrightActive = 1;\nleftActive = 1;\nscaleFactor=1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\n"
+      "FreqDithering=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switc"
+      "h = 1;\na_switch = 0;\ndisp('Init done');"
       Position		      [1762, 173]
       ForegroundColor	      "red"
       DropShadow	      on
       ClickFcn		      "addpath /opt/robot/matlab;\naddpath /home/martinka/Projects/SARAFun/demopert;\naddpath /opt/rob"
       "ot/matlab/irb/mex;\naddpath /opt/robot/matlab/irb/mex/mex_ESMERALDA; \naddpath /home/martinka/Projects/SARAFun/b"
       "p-ar-hmm/dmp/leadthrough;\naddpath /home/martinka/frida_dynamics;\n\nDOF=7;\nh=0.004032;\nhighKiActive = 0;\nkee"
-      "pStill = 0;\nrightActive = 1;\nleftActive = 1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0.3;\nFreqDitherin"
-      "g=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_switch = 1;\na_sw"
-      "itch = 0;\ndisp('Init done');"
+      "pStill = 0;\nrightActive = 1;\nleftActive = 1;\nscaleFactor=1;\n\nfriction = 0.7;\ncontrol = 1;\nAmpDithering=0."
+      "3;\nFreqDithering=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_s"
+      "witch = 1;\na_switch = 0;\ndisp('Init done');"
       UseDisplayTextAsClickCallback on
     }
   }
@@ -15981,7 +15997,7 @@ Stateflow {
     firstJunction	    92
     viewObj		    90
     machine		    1
-    ssIdHighWaterMark	    12
+    ssIdHighWaterMark	    13
     decomposition	    CLUSTER_CHART
     type		    EML_CHART
     firstData		    94
@@ -16005,17 +16021,18 @@ Stateflow {
     decomposition	    CLUSTER_STATE
     eml {
       isEML		      1
-      script		      "function [e, scale]   = fcn(T44_FK1, T44_FK2)\n%#codegen\n\n% Set scale:\nscale = 0.5;\n\n% Resha"
-      "ping:\nT441 = reshape(T44_FK1, 4,4)';\nT442 = reshape(T44_FK2, 4,4)';\n\nR1 = T441(1:3,1:3);\nR2 = T442(1:3,1:3)"
-      ";\n\np1 = T441(1:3,4);\np2 = scale*T442(1:3,4);\n\n% Position:\npersistent delta_p\nif isempty(delta_p)\n    del"
-      "ta_p =  p2 - p1;\nend\nep = p2 - (p1 + delta_p);\n    \n\n% Rotation:\npersistent delta_R\nif isempty(delta_R)\n"
-      "    delta_R = R1'*R2;\nend\nR = R2*(R1*delta_R)';\n\n\nRxx = R(1,1); Rxy = R(1,2); Rxz = R(1,3);\nRyx = R(2,1); "
-      "Ryy = R(2,2); Ryz = R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( trace( R ) + 1 ) / 2;\n\n% c"
-      "heck if w is real. Otherwise, zero it.\nif( imag( w ) > 0 )\n     w = 0;\nend\n\nx1 = ( Rzy - Ryz ) / (4*w);\ny1"
-      " = ( Rxz - Rzx ) / (4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(isnan(q1)) || any(isinf(q1)"
-      ")\n    q1 = zeros(size(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n    q_old = q1;\nend\n\nq2 = -q1;\nif "
-      "(norm(q_old - q1) < norm(q_old-q2)) || norm(q1)<.1\n    q_out = q1;\nelse\n    q_out = q2;\nend\n\n\nq_old = q_o"
-      "ut;\n\ne = [ep ; q_out];\n\n"
+      script		      "function e    = fcn(T44_FK1, T44_FK2, scale)\n%#codegen\n\npersistent old_scale\nif isempty(old_s"
+      "cale)\n    old_scale = scale;\nend\n\n\n\n% Reshaping:\nT441 = reshape(T44_FK1, 4,4)';\nT442 = reshape(T44_FK2, "
+      "4,4)';\n\nR1 = T441(1:3,1:3);\nR2 = T442(1:3,1:3);\n\np1 = T441(1:3,4);\np2 = scale*T442(1:3,4);\n\n% Position:\n"
+      "persistent delta_p\nif isempty(delta_p) || scale ~= old_scale\n    delta_p =  p2 - p1;\nend\nold_scale = scale;\n"
+      "\nep = p2 - (p1 + delta_p);\n    \n \n% Rotation:\npersistent delta_R\nif isempty(delta_R)\n    delta_R = R1'*R2"
+      ";\nend\nR = R2*(R1*delta_R)';\n\n\nRxx = R(1,1); Rxy = R(1,2); Rxz = R(1,3);\nRyx = R(2,1); Ryy = R(2,2); Ryz = "
+      "R(2,3);\nRzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);\n\nw = sqrt( trace( R ) + 1 ) / 2;\n\n% check if w is real. O"
+      "therwise, zero it.\nif( imag( w ) > 0 )\n     w = 0;\nend\n\nx1 = ( Rzy - Ryz ) / (4*w);\ny1 = ( Rxz - Rzx ) / ("
+      "4*w);\nz1 = ( Ryx - Rxy ) / (4*w);\nq1 = [x1; y1; z1];\n\nif any(isnan(q1)) || any(isinf(q1))\n    q1 = zeros(si"
+      "ze(q1));\nend\n\npersistent q_old\nif isempty(q_old)\n    q_old = q1;\nend\n\nq2 = -q1;\nif (norm(q_old - q1) < "
+      "norm(q_old-q2)) || norm(q1)<.1\n    q_out = q1;\nelse\n    q_out = q2;\nend\n\n\nq_old = q_out;\n\ne = [ep ; q_o"
+      "ut];\n\n\n"
       editorLayout	      "100 M4x1[10 5 700 500]"
     }
   }
@@ -16118,7 +16135,7 @@ Stateflow {
     ssIdNumber		    12
     name		    "scale"
     linkNode		    [90 96 0]
-    scope		    OUTPUT_DATA
+    scope		    INPUT_DATA
     machine		    1
     props {
       array {
@@ -21126,13 +21143,12 @@ Stateflow {
       "2_dot];\n\n% Control gains:\n\n% kp_dist = 225; % Can be increased later\nkp_dist = 250;\nkd_dist = 2*sqrt(kp_di"
       "st); % Nominal. Real negative double pole.\n% kp_quat = 160; % Can be increased later\nkp_quat = 250; % 200\nkd_"
       "quat = 2*sqrt(kp_quat); % Derivative gain\n\nkp = [kp_dist*ones(3,1); kp_quat*ones(3,1)];\nkd = [kd_dist*ones(3,"
-      "1); kd_quat*ones(3,1)]; % Derivative gain\n\n\n% Scaling;\nJ2(1:3,:) = scale*J2(1:3,:);\nJ2_dot(1:3,:) = scale*J"
-      "2_dot(1:3,:);\n\n% Implementation according to paper:\nG = [-J1 J2];\n\n\ny_dot = G*q_dot;\nG_dot = [-J1_dot J2_"
-      "dot];\n\n\nM = blkdiag(B1,B2);\nM = M + 0.005*eye(14);\n\n\nC = blkdiag(C1,C2);\n\nGamma = G*pinv(M)*G';\n\n\n\n"
-      "\nustar = pinv(Gamma)*(G*pinv(M)*(C*q_dot)-G_dot*q_dot);\nlambda = ustar - pinv(Gamma)*(kd.*y_dot + kp.*e);\n\n\n"
-      "\nQ1 = -J1'*lambda;\n\n% Undo previous scaling, and scale acceleration.\nJ2(1:3,:) = J2(1:3,:)./scale^2;\nQ2 = J"
-      "2'*lambda;\n\n\n\n\ndiagm = diag(M);\n% % %  Nice for debugging:\n% Q1(1:3)=0;\n% Q1(7) = 0;\n% Q2(1:3)=0;\n% Q2"
-      "(7) = 0;"
+      "1); kd_quat*ones(3,1)]; % Derivative gain\n\n\n% We use a matrix to separate position and orientation\nscale_mat"
+      "rix = diag([scale,scale,scale,1,1,1]);\n\n\n% Implementation according to paper:\nG = [-J1 scale_matrix*J2];\n\n"
+      "\ny_dot = G*q_dot;\nG_dot = [-J1_dot scale_matrix*J2_dot];\n\n\nM = blkdiag(B1,B2);\nM = M + 0.005*eye(14);\n\n\n"
+      "C = blkdiag(C1,C2);\n\nGamma = G*pinv(M)*G';\n\n\n\n\nustar = pinv(Gamma)*(G*pinv(M)*(C*q_dot)-G_dot*q_dot);\nla"
+      "mbda = ustar - pinv(Gamma)*(kd.*y_dot + kp.*e);\n\n\n\nQ1 = -J1'*lambda;\n\n\nQ2 = J2'*lambda;\n\n\n\n\ndiagm = "
+      "diag(M);\n% % %  Nice for debugging:\n% Q1(1:3)=0;\n% Q1(7) = 0;\n% Q2(1:3)=0;\n% Q2(7) = 0;"
       editorLayout	      "100 M4x1[10 5 700 500]"
     }
   }