Skip to content

GitLab

Commit 2802c49e authored by Martin Karlsson's avatar Martin Karlsson
Browse files

hej

parent edc6ce53
Hide whitespace changes
Inline Side-by-side
Showing with 86 additions and 70 deletions
scaled_teleoperation.mdl
View file @ 2802c49e
......@@ -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]"
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment