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]" } }