Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Martin Karlsson
teleoperation_extctrl_simulink
Commits
2802c49e
Commit
2802c49e
authored
Sep 26, 2019
by
Martin Karlsson
Browse files
hej
parent
edc6ce53
Changes
1
Hide whitespace changes
Inline
Side-by-side
scaled_teleoperation.mdl
View file @
2802c49e
...
...
@@ -384,7 +384,7 @@ Model {
Name "ext2irb.robot[0].mocgendata.string2[i]"
}
ParameterArgumentNames ""
ComputedModelVersion "1.28
66
"
ComputedModelVersion "1.28
87
"
NumModelReferences 0
NumTestPointedSignals 21
TestPointedSignal {
...
...
@@ -496,9 +496,9 @@ Model {
ModifiedByFormat "%<Auto>"
LastModifiedBy "martinka"
ModifiedDateFormat "%<Auto>"
LastModifiedDate "Tu
e
Sep
17 20:31:39
2019"
RTWModifiedTimeStamp 49
0651023
ModelVersionFormat "1.%<AutoIncrement:28
66
>"
LastModifiedDate "T
h
u Sep
26 17:10:10
2019"
RTWModifiedTimeStamp 49
1418270
ModelVersionFormat "1.%<AutoIncrement:28
87
>"
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, 11
50
]
CurrentDlgPage "
Optimization/Signals and Parameters
"
ConfigPrmDlgPosition [ 427,
64
, 1493, 11
87
]
}
PropName "ConfigurationSets"
}
...
...
@@ -1345,7 +1345,7 @@ Model {
ShowPageBoundaries off
ZoomFactor "100"
ReportName "simulink-default.rpt"
SIDHighWatermark "142
1
"
SIDHighWatermark "142
2
"
Block {
BlockType Inport
Name "irb2ext.obtaining"
...
...
@@ -5189,7 +5189,7 @@ Model {
MaskHideContents off
System {
Name "Right"
Location [0,
-28
, 1920, 1
080
]
Location [0,
46
, 1920, 1
154
]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
...
...
@@ -7907,7 +7907,7 @@ Model {
System {
Name "botharms_get_torque"
Location [0, 18, 1920, 1126]
Open o
n
Open o
ff
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
...
...
@@ -7919,7 +7919,7 @@ Model {
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "49
7
"
SIDHighWatermark "49
8
"
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 [7
4
0, 818,
79
0, 922]
Ports [
3
,
1
]
Position [7
5
0, 818,
80
0, 922]
ZOrder -4
LibraryVersion "1.32"
ErrorFcn "Stateflow.Translate.translate"
...
...
@@ -9622,7 +9631,7 @@ Model {
TiledPageScale 1
ShowPageBoundaries off
ZoomFactor "100"
SIDHighWatermark "2
7
"
SIDHighWatermark "2
9
"
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 [
19
5, 0]
Points [
20
5, 0]
DstBlock "MATLAB Function1"
DstPort 1
}
...
...
@@ -12544,7 +12545,7 @@ Model {
SrcPort 1
Points [10, 0]
Branch {
Points [195, 0; 0, 2
7
5]
Points [195, 0; 0, 25
0
]
DstBlock "MATLAB Function1"
DstPort 2
}
...
...
@@ -12673,7 +12674,7 @@ Model {
Labels [0, 0]
SrcBlock "MATLAB Function1"
SrcPort 1
Points [
2
0, 0]
Points [
1
0, 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;\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');"
"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_s
witch = 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;\nFreqDitherin
g=15;\nadmit_CTRL=0;\n\n\n[B_lpfilt,A_lpfilt] = butter(1,0.01);\n\nparametersLeadthrough;\n\nf_s"
"
witch = 1;\na_sw
itch = 0;\ndisp('Init done');"
UseDisplayTextAsClickCallback on
}
}
...
...
@@ -15981,7 +15997,7 @@ Stateflow {
firstJunction 92
viewObj 90
machine 1
ssIdHighWaterMark 1
2
ssIdHighWaterMark 1
3
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
OUT
PUT_DATA
scope
IN
PUT_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]"
}
}
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment