diff --git a/implementation/SCARA/04_path_planning/implementation.md b/implementation/SCARA/04_path_planning/implementation.md new file mode 100644 index 0000000..905dedf --- /dev/null +++ b/implementation/SCARA/04_path_planning/implementation.md @@ -0,0 +1,7 @@ +# Path Planning +The current path planning makes it that there is an infinite acceleration required in the corners. +We need something that can process setpoints in a way that it can follow those. + +## General Idea. +So if we want to draw a box. 4 setpoints for each corner should be sufficient. +This is a simple model detail. We just take the four setpoint and give the system some time to get there. diff --git a/implementation/SCARA/04_path_planning/path_planning.emx b/implementation/SCARA/04_path_planning/path_planning.emx new file mode 100644 index 0000000..ef4617d --- /dev/null +++ b/implementation/SCARA/04_path_planning/path_planning.emx @@ -0,0 +1,5299 @@ + + + + + + 4.8 +0 + False + C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\04_path_planning\path_planning.emx + 2020-7-20 17:47:38 +True + + + parameters + real A = 0.05 {m}; + real B = 0.05 {m}; +variables + real J0_1_BF1[2]; + real J1_2_BF1[2]; + real J1_2_BF2[2]; + real EE1_BF2[2]; +initialequations + J0_1_BF1 = [A/2;0]; + J1_2_BF1 = [-A/2;0]; + J1_2_BF2 = [B/2;0]; + EE1_BF2 = [-B/2;0]; + +'; + type Mainmodel + end; + implementation bg + submodels + Base1 720 184 + description '4.81FalseTrueBond Graph\MR\center_of_mass_v2.emx2020-7-20 15:56:10Baseparameters + real I [3,1] = [7.583333333333335e-7; 3.645833333333334e-8; 7.364583333333335e-7] {N.m.s}; + real m = 0.0035 {kg};'; + type Submodel + ports + power in p [6,1]; + signal in Hin [4,4]; + signal out Hout [4,4]; + end; + icon bg ellipse bottom + figures + ellipse 696 160 744 208 color 0 fill 16777215 width 2; + line 696 184 744 184 color 0 fill 16777215; + line 720 160 720 208 color 0 fill 16777215; + terminals + p 704 160 fixed; + Hin 736 160 fixed; + Hout 744 216 fixed; + end; + implementation bg + submodels + AdHi0 544 400 + description '4.01False2007-9-25 12:3:3True'; + type MTF + ports + power in p1 [6,1]; + power out p2 [6,1]; + signal in H [4,4]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg left + figures + text 'MTF' 544 400 color 0 18 bold; + end; + implementation eq +variables + real onlyRotH[4,4]; +code + //Only rotations with respect to the inertial system matter! + onlyRotH = H; + onlyRotH[1,4] = 0; + onlyRotH[2,4] = 0; + onlyRotH[3,4] = 0; + + p2.e = transpose(Adjoint(onlyRotH)) * p1.e; + p1.f = Adjoint(onlyRotH) * p2.f; +implementation_end; + EJS 616 336 + description '4.01False2007-10-31 11:43:6True'; + type MGY + ports + power in p1 [6,1]; + end; + icon bg top + figures + text 'MGY' 616 336 color 0 18 bold; + end; + implementation eq +//EJS / Gyroscopic effects +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Q[6,6]; + real QI[6,6]; + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + Q = transpose(adjoint(p1.f)); + QI = Q*II; + p1.e = QI*p1.f;implementation_end; + Gravity 664 400 + description '4.01False2007-9-25 12:3:26True'; + type Se + ports + power out p [6,1]; + restrictions + causality fixed out p; + end; + icon bg bottom + figures + text 'Se' 664 400 color 0 18 bold; + end; + implementation eq +parameters + real global m; +variables + real effort[6]; +equations + effort = [0;0;0;0;0;-g_n*m]; + p.e = effort; +implementation_end; + InertialTensor 544 288 + description '4.01False2007-9-25 12:2:12True'; + type I + ports + power in p [6,1]; + signal out state [6,1]; + restrictions + causality preferred in p; + end; + icon bg top + figures + text 'I' 544 288 color 0 18 bold; + end; + implementation eq +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + state = int(p.e); //state = generalized momentum + p.f = inverse(II)*state; +implementation_end; + plug p 472 336; + plug Hin 472 496; + plug Hout 640 496; + Splitter1 544 496 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; + ellipse 539.7 491.7 548.3 500.3 color -1; + terminals + input 544 496 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Ta0j 544 336 + description ' + 4.0 + 1 + False + Bond Graph\OneJunction.emx + 2007-9-27 9:51:18 + True + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg bottom + figures + text '1' 544 336 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + ZeroJunction1 504 336 + description ' + + 4.2 +1 + False + Bond Graph\ZeroJunction.emx + 2011-11-29 16:45:16 + +'; + knot ZeroJunction + ports + power knot duplicatable none p [6,1]; + signal knot out effort [6,1]; + restrictions + causality constraint one_in p; + end; + icon bg + figures + text '0' 504 336 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.f)) = 0; + equal (collect (p.e)); + effort = first (p.e); +implementation_end; + end; + connections + AdHi0\p2 => Ta0j\p; + Gravity\p => AdHi0\p1; + Hin -> Splitter1\input; + InertialTensor\p <= Ta0j\p; + p => ZeroJunction1\p; + Splitter1\output -> AdHi0\H; + Splitter1\output -> Hout; + Ta0j\p => EJS\p1; + Ta0j\p => ZeroJunction1\p; + end; + parameterrelations +InertialTensor\I = I; +InterialTensor\m = m; +EJS\I = I; +EJS\m = m; +AdHik\COMdim = COMdim; +AdHik1\COMdim = COMdim; +Hij\dim = dim; +Gravity\m=m;parameterrelations_end; + figures + text 'b = current link (body) +a = previous link (body) +i = Body fixed frame, fixed in joint with previous link +j = Body fixed frame, fixed in joint with next link +k = Body fixed frame, principal inertial frame +0 = inertial system + ' 288 280 color 0; + implementation_end; + Base2 936 120 + description '4.81FalseTrueBond Graph\MR\center_of_mass_v2.emx2020-7-20 15:56:10Baseparameters + real I [3,1] = [0.0001;0.0001;0.0001] {N.m.s}; + real m = 0.015 {kg};'; + type Submodel + ports + power in p [6,1]; + signal in Hin [4,4]; + signal out Hout [4,4]; + end; + icon bg ellipse bottom + figures + ellipse 912 96 960 144 color 0 fill 16777215 width 2; + line 936 96 936 144 color 0 fill 16777215; + line 912 120 960 120 color 0 fill 16777215; + terminals + p 912 104 fixed; + Hin 912 136 fixed; + Hout 968 144 fixed; + end; + implementation bg + submodels + AdHi0 544 400 + description '4.01False2007-9-25 12:3:3True'; + type MTF + ports + power in p1 [6,1]; + power out p2 [6,1]; + signal in H [4,4]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg left + figures + text 'MTF' 544 400 color 0 18 bold; + end; + implementation eq +variables + real onlyRotH[4,4]; +code + //Only rotations with respect to the inertial system matter! + onlyRotH = H; + onlyRotH[1,4] = 0; + onlyRotH[2,4] = 0; + onlyRotH[3,4] = 0; + + p2.e = transpose(Adjoint(onlyRotH)) * p1.e; + p1.f = Adjoint(onlyRotH) * p2.f; +implementation_end; + EJS 616 336 + description '4.01False2007-10-31 11:43:6True'; + type MGY + ports + power in p1 [6,1]; + end; + icon bg top + figures + text 'MGY' 616 336 color 0 18 bold; + end; + implementation eq +//EJS / Gyroscopic effects +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Q[6,6]; + real QI[6,6]; + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + Q = transpose(adjoint(p1.f)); + QI = Q*II; + p1.e = QI*p1.f;implementation_end; + Gravity 616 400 + description '4.01False2007-9-25 12:3:26True'; + type Se + ports + power out p [6,1]; + restrictions + causality fixed out p; + end; + icon bg bottom + figures + text 'Se' 616 400 color 0 18 bold; + end; + implementation eq +parameters + real global m; +variables + real effort[6]; +equations + effort = [0;0;0;0;0;-g_n*m]; + p.e = effort; +implementation_end; + InertialTensor 544 288 + description '4.01False2007-9-25 12:2:12True'; + type I + ports + power in p [6,1]; + signal out state [6,1]; + restrictions + causality preferred in p; + end; + icon bg top + figures + text 'I' 544 288 color 0 18 bold; + end; + implementation eq +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + state = int(p.e); //state = generalized momentum + p.f = inverse(II)*state; +implementation_end; + plug p 472 336; + plug Hin 472 496; + plug Hout 640 496; + Splitter1 544 496 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; + ellipse 539.7 491.7 548.3 500.3 color -1; + terminals + input 544 496 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Ta0j 544 336 + description ' + 4.0 + 1 + False + Bond Graph\OneJunction.emx + 2007-9-27 9:51:18 + True + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg bottom + figures + text '1' 544 336 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + end; + connections + AdHi0\p2 => Ta0j\p; + Gravity\p => AdHi0\p1; + Hin -> Splitter1\input; + InertialTensor\p <= Ta0j\p; + p => Ta0j\p; + Splitter1\output -> AdHi0\H; + Splitter1\output -> Hout; + Ta0j\p => EJS\p1; + end; + parameterrelations +InertialTensor\I = I; +InterialTensor\m = m; +EJS\I = I; +EJS\m = m; +AdHik\COMdim = COMdim; +AdHik1\COMdim = COMdim; +Hij\dim = dim; +Gravity\m=m;parameterrelations_end; + figures + text 'b = current link (body) +a = previous link (body) +i = Body fixed frame, fixed in joint with previous link +j = Body fixed frame, fixed in joint with next link +k = Body fixed frame, principal inertial frame +0 = inertial system + ' 288 280 color 0; + implementation_end; + Base3 376 184 + description ' + + 4.8 +1 + False + True + Bond Graph\MR\center_of_mass_v2.emx + 2020-7-20 15:56:10 +Base + + + parameters + real I [3,1] = [1.6399999999999998e-6; 4.7e-8; 1.61e-6] {N.m.s}; + real m = 0.00455 {kg}; + +'; + type Submodel + ports + power in p [6,1]; + signal in Hin [4,4]; + signal out Hout [4,4]; + end; + icon bg ellipse bottom + figures + ellipse 352 160 400 208 color 0 fill 16777215 width 2; + line 352 184 400 184 color 0 fill 16777215; + line 376 160 376 208 color 0 fill 16777215; + terminals + p 360 160 fixed; + Hin 392 160 fixed; + end; + implementation bg + submodels + AdHi0 544 400 + description '4.01False2007-9-25 12:3:3True'; + type MTF + ports + power in p1 [6,1]; + power out p2 [6,1]; + signal in H [4,4]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg left + figures + text 'MTF' 544 400 color 0 18 bold; + end; + implementation eq +variables + real onlyRotH[4,4]; +code + //Only rotations with respect to the inertial system matter! + onlyRotH = H; + onlyRotH[1,4] = 0; + onlyRotH[2,4] = 0; + onlyRotH[3,4] = 0; + + p2.e = transpose(Adjoint(onlyRotH)) * p1.e; + p1.f = Adjoint(onlyRotH) * p2.f; +implementation_end; + EJS 616 336 + description '4.01False2007-10-31 11:43:6True'; + type MGY + ports + power in p1 [6,1]; + end; + icon bg top + figures + text 'MGY' 616 336 color 0 18 bold; + end; + implementation eq +//EJS / Gyroscopic effects +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Q[6,6]; + real QI[6,6]; + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + Q = transpose(adjoint(p1.f)); + QI = Q*II; + p1.e = QI*p1.f;implementation_end; + Gravity 656 400 + description '4.01False2007-9-25 12:3:26True'; + type Se + ports + power out p [6,1]; + restrictions + causality fixed out p; + end; + icon bg bottom + figures + text 'Se' 656 400 color 0 18 bold; + end; + implementation eq +parameters + real global m; +variables + real effort[6]; +equations + effort = [0;0;0;0;0;-g_n*m]; + p.e = effort; +implementation_end; + InertialTensor 544 288 + description '4.01False2007-9-25 12:2:12True'; + type I + ports + power in p [6,1]; + signal out state [6,1]; + restrictions + causality preferred in p; + end; + icon bg top + figures + text 'I' 544 288 color 0 18 bold; + end; + implementation eq +parameters + real global I[3]; + real global m; +variables + real II[6,6]; //Inertial tensor + real Ia[6]; +initialequations + Ia[1:3] = I; + Ia[4:6] = m; + II = diag(Ia); +equations + state = int(p.e); //state = generalized momentum + p.f = inverse(II)*state; +implementation_end; + plug p 472 336; + plug Hin 472 496; + plug Hout 640 496; + Splitter1 544 496 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; + ellipse 539.7 491.7 548.3 500.3 color -1; + terminals + input 544 496 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Ta0j 544 336 + description ' + 4.0 + 1 + False + Bond Graph\OneJunction.emx + 2007-9-27 9:51:18 + True + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg bottom + figures + text '1' 544 336 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + ZeroJunction1 504 336 + description ' + + 4.2 +1 + False + Bond Graph\ZeroJunction.emx + 2011-11-29 16:45:16 + +'; + knot ZeroJunction + ports + power knot duplicatable none p [6,1]; + signal knot out effort [6,1]; + restrictions + causality constraint one_in p; + end; + icon bg + figures + text '0' 504 336 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.f)) = 0; + equal (collect (p.e)); + effort = first (p.e); +implementation_end; + end; + connections + AdHi0\p2 => Ta0j\p; + Gravity\p => AdHi0\p1; + Hin -> Splitter1\input; + InertialTensor\p <= Ta0j\p; + p => ZeroJunction1\p; + Splitter1\output -> AdHi0\H; + Splitter1\output -> Hout; + Ta0j\p => EJS\p1; + Ta0j\p => ZeroJunction1\p; + end; + parameterrelations +InertialTensor\I = I; +InterialTensor\m = m; +EJS\I = I; +EJS\m = m; +AdHik\COMdim = COMdim; +AdHik1\COMdim = COMdim; +Hij\dim = dim; +Gravity\m=m;parameterrelations_end; + figures + text 'b = current link (body) +a = previous link (body) +i = Body fixed frame, fixed in joint with previous link +j = Body fixed frame, fixed in joint with next link +k = Body fixed frame, principal inertial frame +0 = inertial system + ' 288 280 color 0; + implementation_end; + C1 640 264 + description '4.01False + Bond Graph\3D\C-3.emx + 2007-9-25 12:12:8 +'; + type 'C-3' + ports + power in p [3,1]; + signal out state [3,1]; + restrictions + causality preferred out p; + end; + icon bg bottom + figures + text 'C' 640 264 color 0 18 bold; + end; + implementation eq +parameters + real c[3,3] = [0.1, 0.0, 0.0; 0.0, 0.1, 0.0; 0.0, 0.0, 0.1] {mN/m}; +equations + state = int(p.f); + p.e = inverse(c) *state; +implementation_end; + C2 304 272 + description '4.01False + Bond Graph\3D\C-3.emx + 2007-9-25 12:12:8 +'; + type 'C-3' + ports + power in p [3,1]; + signal out state [3,1]; + restrictions + causality preferred out p; + end; + icon bg bottom + figures + text 'C' 304 272 color 0 18 bold; + end; + implementation eq +parameters + real c[3,3] = [0.1, 0.0, 0.0; 0.0, 0.1, 0.0; 0.0, 0.0, 0.1] {mN/m}; +equations + state = int(p.f); + p.e = inverse(c) *state; +implementation_end; + C3 304 224 + description '4.01False + Bond Graph\2D\C-2.emx + 2007-9-25 12:7:27 +'; + type 'C-2' + ports + power in p [2,1]; + signal out state [2,1]; + restrictions + causality preferred out p; + end; + icon bg bottom + figures + text 'C' 304 224 color 0 18 bold; + end; + implementation eq +parameters + real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; +equations + state = int(p.f); + p.e = inverse(c) *state; +implementation_end; + C4 640 216 + description '4.01False + Bond Graph\2D\C-2.emx + 2007-9-25 12:7:27 +'; + type 'C-2' + ports + power in p [2,1]; + signal out state [2,1]; + restrictions + causality preferred out p; + end; + icon bg bottom + figures + text 'C' 640 216 color 0 18 bold; + end; + implementation eq +parameters + real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; +equations + state = int(p.f); + p.e = inverse(c) *state; +implementation_end; + C5 472 216 + description ' + + 4.2 +1 + False + Bond Graph\C.emx + 2011-11-29 15:58:35 + +'; + type C + ports + power in p; + signal out state; + restrictions + causality preferred out p; + end; + icon bg bottom + figures + text 'C' 472 216 color 0 18 bold; + end; + implementation eq +parameters + real c = 0.0001; +equations + state = int(p.f); + p.e = state / c; +implementation_end; + Integrate 184 288 + description ' + + 4.3 +1 + False + Signal\Block Diagram\Integrate.emx + 2013-3-8 14:47:42 + +'; + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 168 272 200 304 color 0 fill 15132390; + text '∫' 184 288.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real initial = 0; // initial value +equations + output = int (input, initial); + implementation_end; + Integrate1 520 360 + description ' + + 4.3 +1 + False + Signal\Block Diagram\Integrate.emx + 2013-3-8 14:47:42 + +'; + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 504 344 536 376 color 0 fill 15132390; + text '∫' 520 360.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real initial = 0; // initial value +equations + output = int (input, initial); + implementation_end; + inverse_kinematics1 272 552 + description ' + + 4.8 + SCARA\inverse_kinematics_v1.emx +1 + False + 2020-7-10 12:26:18 +False + +'; + type 'Submodel-Equation' + ports + signal in input [2,1] {m} ; + signal out angle1 {rad} ; + signal out angle2 {rad} ; + signal out a {rad} ; + signal out b {rad} ; + signal out c {rad} ; + signal out phi {rad} ; + signal out abs_angle2; + end; + implementation eq +parameters + real A_length = 0.065 {m}; // length of first arm + real B_length = 0.05 {m}; // length of second arm + real to_rad = 1 {rad}; +variables + real x {m}, y {m}; + real C_length {m}; // length to x and y. + real angle_sum {rad}; // test angle +equations + x = input[1]; + y = input[2]; + phi = atan2(y, x); + C_length = sqrt(x^2 + y^2); + a = arccos ((B_length^2 + C_length^2 - A_length^2) / (2 * B_length * C_length)); + b = arccos ((A_length^2 + C_length^2 - B_length^2) / (2 * A_length * C_length)); + c = arccos ((A_length^2 + B_length^2 - C_length^2) / (2 * A_length * B_length)); + angle1 = b + phi; + abs_angle2 = angle1 - pi * to_rad + c; + angle2 = c - pi * to_rad; + + + + implementation_end; + Joint1 200 120 + description ' + + 4.8 +1 + Bond Graph\MR\joint-v3.emx + 2020-7-20 14:25:50 + +'; + type 'Submodel-v3' + ports + power in Pin [6,1]; + signal in Hin [4,4]; + power in Pdiff [6,1]; + signal out Hout [4,4]; + power out Pout [6,1]; + end; + icon bg bottom + figures + rectangle 176 88 224 152 color 0 fill 14745599; + text 'Joint' 200 120 color 0 'Clear Sans' 16; + terminals + Pin 224 104 fixed; + Hin 176 136 fixed; + Hout 224 136 fixed; + Pout 176 104 fixed; + end; + implementation bg + submodels + AdHji 424 424 + description ' + 4.0 + 1 + False + Bond Graph\MTF.emx + 2007-9-25 12:3:3 + True +'; + type MTF + ports + power out p1 [6,1]; + power in p2 [6,1]; + signal in H [4,4]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg bottom + figures + text 'MTF' 424 424 color 0 18 bold; + end; + implementation eq +equations + p2.e = transpose(Adjoint(H)) * p1.e; + p1.f = Adjoint(H) * p2.f;implementation_end; + FlowSensor2 240 271.9 + description ' + + 4.2 +1 + False + Bond Graph\FlowSensor.emx + 2011-11-29 15:50:53 + +'; + knot FlowSensor + ports + power knot in p1 [6,1]; + power knot out p2 [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg ellipse + figures + ellipse 233.1 264.8 246.9 279.1 color 0 fill 16777215; + text 'f' 240 271.2 color 0; + end; + implementation eq +equations + p2.f = p1.f; + p1.e = p2.e; + flow = p1.f; +implementation_end; + Hmatrix 320 272 + description '4.0Template\Submodel-Equation.emx1False2007-11-1 22:32:1False'; + type 'Submodel-Equation' + ports + signal in flow [6,1]; + signal out H [4,4]; + end; + implementation eq +parameters + real init[4] = [1;0;0;0]; +variables + real q[4]; //quaternions + real W[3,4]; //Quaternion Rates Matrix + real R[3,3]; //Rotation Matrix + real p[3]; //Position Vector + real dq[4]; +equations + dq = transpose(W) * flow[1:3] ./ 2; + q = int(dq,init); + p = int(flow[4:6]); + W = [-q[2], q[1], -q[4], q[3]; + -q[3], q[4], q[1], -q[2]; + -q[4], -q[3], q[2], q[1]]; + R = [q[1]^2+q[2]^2-q[3]^2-q[4]^2, 2*(q[2]*q[3]+q[1]*q[4]), 2*(q[2]*q[4]-q[1]*q[3]); + 2*(q[2]*q[3]-q[1]*q[4]), q[1]^2-q[2]^2+q[3]^2-q[4]^2, 2*(q[3]*q[4]+q[1]*q[2]); + 2*(q[2]*q[4]+q[1]*q[3]), 2*(q[3]*q[4]-q[1]*q[2]), q[1]^2-q[2]^2-q[3]^2+q[4]^2]; + H = homogeneous(R,p); + implementation_end; + MatrixMul 320 576 + description ' + 4.0 + 1 + False + Signal\Block Diagram\Gain.emx + 2007-9-26 12:15:12 + True +'; + type Gain + ports + signal in input1 [4,4]; + signal out output [4,4]; + signal in input2 [4,4]; + end; + icon bg bottom + figures + rectangle 304.1 560 335.9 592 color 0 fill 15132390; + text 'X' 320 576 color 16711680 16 bold; + end; + implementation eq +equations + output = input2*input1; implementation_end; + plug Hin 121.4 576; + plug Pdiff 240 220; + plug Hout 491.2 576; + plug Pout 119.4 424; + plug Pin 485.6 424; + Splitter2 320 312 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 316.8 308.8 323.2 315.2 color -1 fill 0; + ellipse 315.7 307.7 324.3 316.3 color -1; + terminals + input 320 312 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Wbai 240 424 + description ' + 4.0 + 1 + False + Bond Graph\ZeroJunction.emx + 2007-9-27 9:51:43 + True +'; + knot ZeroJunction + ports + power knot duplicatable none p [6,1]; + signal knot out effort [6,1]; + restrictions + causality constraint one_in p; + end; + icon bg bottom + figures + text '0' 240 424 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.f)) = 0; + equal (collect (p.e)); + effort = first (p.e); +implementation_end; + end; + connections + FlowSensor2\flow -> Hmatrix\flow; + FlowSensor2\p2 => Wbai\p; + Hin -> MatrixMul\input2; + Hmatrix\H -> Splitter2\input; + MatrixMul\output -> Hout; + Pdiff => FlowSensor2\p1; + Pin => AdHji\p2; + Splitter2\output -> AdHji\H 424 312; + Splitter2\output -> MatrixMul\input1; + Wbai\p <= AdHji\p1; + Wbai\p => Pout; + end; + parameterrelations +EndstopMin\Rendstop = Rendstop; +EndstopMin\Cendstop = Cendstop; +EndstopMin\InitialPos = InitialPos; +EndstopMin\EndstopPos = MinEndstopPos; +EndstopMax\Rendstop = Rendstop; +EndstopMax\Cendstop = Cendstop; +EndstopMax\InitialPos = InitialPos; +EndstopMax\EndstopPos = MaxEndstopPos; +Rjoint\Rjoint= Rjoint; +Integrate\init = InitialPos; +uTbai\Rconstraint = Rconstraint; +uTbai\Cconstraint = Cconstraint; + +parameterrelations_end; + implementation_end; + Joint2 536 120 + description ' + + 4.8 +1 + Bond Graph\MR\joint-v3.emx + 2020-7-20 14:25:50 + +'; + type 'Submodel-v3' + ports + power in Pin [6,1]; + signal in Hin [4,4]; + power in Pdiff [6,1]; + signal out Hout [4,4]; + power out Pout [6,1]; + signal out Hdiff [4,4]; + end; + icon bg bottom + figures + rectangle 512 88 560 152 color 0 fill 14745599; + text 'Joint' 536 120 color 0 'Clear Sans' 16; + terminals + Pin 560 104 fixed; + Hin 512 136 fixed; + Hout 560 136 fixed; + Pout 512 104 fixed; + end; + implementation bg + submodels + AdHji 424 424 + description ' + 4.0 + 1 + False + Bond Graph\MTF.emx + 2007-9-25 12:3:3 + True +'; + type MTF + ports + power out p1 [6,1]; + power in p2 [6,1]; + signal in H [4,4]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg bottom + figures + text 'MTF' 424 424 color 0 18 bold; + end; + implementation eq +equations + p2.e = transpose(Adjoint(H)) * p1.e; + p1.f = Adjoint(H) * p2.f;implementation_end; + FlowSensor2 184 311.9 + description ' + + 4.2 +1 + False + Bond Graph\FlowSensor.emx + 2011-11-29 15:50:53 + +'; + knot FlowSensor + ports + power knot in p1 [6,1]; + power knot out p2 [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint not_equal p1 p2; + end; + icon bg ellipse + figures + ellipse 177.1 304.8 190.9 319.1 color 0 fill 16777215; + text 'f' 184 311.2 color 0; + end; + implementation eq +equations + p2.f = p1.f; + p1.e = p2.e; + flow = p1.f; +implementation_end; + Hmatrix 256 312 + description '4.0Template\Submodel-Equation.emx1False2007-11-1 22:32:1False'; + type 'Submodel-Equation' + ports + signal in flow [6,1]; + signal out H [4,4]; + end; + implementation eq +parameters + real init[4] = [1;0;0;0]; +variables + real q[4]; //quaternions + real W[3,4]; //Quaternion Rates Matrix + real R[3,3]; //Rotation Matrix + real p[3]; //Position Vector +equations + ddt(q,init) = transpose(W) * flow[1:3] ./ 2; + p = int(flow[4:6]); + W = [-q[2], q[1], -q[4], q[3]; + -q[3], q[4], q[1], -q[2]; + -q[4], -q[3], q[2], q[1]]; + //R = dll('EulerAngles.dll','RotationMatrixFromQuaternion',q); + R = [q[1]^2+q[2]^2-q[3]^2-q[4]^2, 2*(q[2]*q[3]+q[1]*q[4]), 2*(q[2]*q[4]-q[1]*q[3]); + 2*(q[2]*q[3]-q[1]*q[4]), q[1]^2-q[2]^2+q[3]^2-q[4]^2, 2*(q[3]*q[4]+q[1]*q[2]); + 2*(q[2]*q[4]+q[1]*q[3]), 2*(q[3]*q[4]-q[1]*q[2]), q[1]^2-q[2]^2-q[3]^2+q[4]^2]; + H = homogeneous(R,p); + + implementation_end; + MatrixMul 320 576 + description ' + 4.0 + 1 + False + Signal\Block Diagram\Gain.emx + 2007-9-26 12:15:12 + True +'; + type Gain + ports + signal in input1 [4,4]; + signal out output [4,4]; + signal in input2 [4,4]; + end; + icon bg bottom + figures + rectangle 304.1 560 335.9 592 color 0 fill 15132390; + text 'X' 320 576 color 16711680 16 bold; + end; + implementation eq +equations + output = input2*input1; implementation_end; + plug Pin 485.6 424; + plug Hin 121.4 576; + plug Pdiff 184 216; + plug Hout 491.2 576; + plug Pout 119.4 424; + plug Hdiff 320 216; + Splitter2 320 312 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 316.8 308.8 323.2 315.2 color -1 fill 0; + ellipse 315.7 307.7 324.3 316.3 color -1; + terminals + input 320 312 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Wbai 184 424 + description ' + 4.0 + 1 + False + Bond Graph\ZeroJunction.emx + 2007-9-27 9:51:43 + True +'; + knot ZeroJunction + ports + power knot duplicatable none p [6,1]; + signal knot out effort [6,1]; + restrictions + causality constraint one_in p; + end; + icon bg bottom + figures + text '0' 184 424 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.f)) = 0; + equal (collect (p.e)); + effort = first (p.e); +implementation_end; + end; + connections + FlowSensor2\flow -> Hmatrix\flow; + FlowSensor2\p2 => Wbai\p; + Hin -> MatrixMul\input2; + Hmatrix\H -> Splitter2\input; + MatrixMul\output -> Hout; + Pdiff => FlowSensor2\p1; + Pin => AdHji\p2; + Splitter2\output -> AdHji\H 424 312; + Splitter2\output -> Hdiff; + Splitter2\output -> MatrixMul\input1; + Wbai\p <= AdHji\p1; + Wbai\p => Pout; + end; + parameterrelations +EndstopMin\Rendstop = Rendstop; +EndstopMin\Cendstop = Cendstop; +EndstopMin\InitialPos = InitialPos; +EndstopMin\EndstopPos = MinEndstopPos; +EndstopMax\Rendstop = Rendstop; +EndstopMax\Cendstop = Cendstop; +EndstopMax\InitialPos = InitialPos; +EndstopMax\EndstopPos = MaxEndstopPos; +Rjoint\Rjoint= Rjoint; +Integrate\init = InitialPos; +uTbai\Rconstraint = Rconstraint; +uTbai\Cconstraint = Cconstraint; + +parameterrelations_end; + implementation_end; + Link 288 120 + description ' + + 4.8 + Bond Graph\MR\link-v3.emx +1 + False + 2020-7-19 15:22:34 +False + +'; + type 'Submodel-Equation' + ports + signal in Hin [4,4]; + signal out Hout [4,4]; + power in Pin [6,1]; + power out Pout [6,1]; + restrictions + causality constraint not_equal Pin Pout; + end; + icon bg bottom + figures + rectangle 256 96 320 144 color 0 fill 8454041; + text 'Link' 288 120 color 0 'Clear Sans' 16; + terminals + Hin 256 136 fixed; + Hout 320 136 fixed; + Pin 320 104 fixed; + Pout 256 104 fixed; + end; + implementation eq +parameters + real offset[3]= [0;0.0325;0]; //coordinates of joint_1 +variables + real Hab[4,4]; + real AdHab[6,6]; +equations + Hab = homogeneous(eye(3),offset); + AdHab = Adjoint(eye(3),offset); + Hout = Hin * Hab; + Pout.e = transpose(AdHab) * Pin.e; + Pin.f = AdHab * Pout.f; + + implementation_end; + Link1 456 120 + description ' + + 4.8 + Bond Graph\MR\link-v3.emx +1 + False + 2020-7-19 15:22:34 +False + +'; + type 'Submodel-Equation' + ports + signal in Hin [4,4]; + signal out Hout [4,4]; + power in Pin [6,1]; + power out Pout [6,1]; + restrictions + causality constraint not_equal Pin Pout; + end; + icon bg bottom + figures + rectangle 424 96 488 144 color 0 fill 8454041; + text 'Link' 456 120 color 0 'Clear Sans' 16; + terminals + Hin 424 136 fixed; + Hout 488 136 fixed; + Pin 488 104 fixed; + Pout 424 104 fixed; + end; + implementation eq +parameters + real offset[6]= [0;0;0;0;0.0325;0]; //coordinates of joint_1 +variables + real Hab[4,4]; + real AdHab[6,6]; + real R[3,3]; + real omega[3]; +initialequations + omega = offset[1:3]; + R = dll('EulerAngles.dll','RotationMatrixFromEulXYZs',omega); + Hab = homogeneous(R,offset[4:6]); + AdHab = Adjoint(Hab); +equations + Hout = Hin * Hab; + Pout.e = transpose(AdHab) * Pin.e; + Pin.f = AdHab * Pout.f; + + implementation_end; + Link2 624 120 + description ' + + 4.8 + Bond Graph\MR\link-v3.emx +1 + False + 2020-7-19 15:22:34 +False + +'; + type 'Submodel-Equation' + ports + signal in Hin [4,4]; + signal out Hout [4,4]; + power in Pin [6,1]; + power out Pout [6,1]; + restrictions + causality constraint not_equal Pin Pout; + end; + icon bg bottom + figures + rectangle 592 96 656 144 color 0 fill 8454041; + text 'Link' 624 120 color 0 'Clear Sans' 16; + terminals + Hin 592 136 fixed; + Hout 656 136 fixed; + Pin 656 104 fixed; + Pout 592 104 fixed; + end; + implementation eq +parameters + real offset[3]= [0;0.025;0]; //coordinates of joint_1 +variables + real Hab[4,4]; + real AdHab[6,6]; +equations + Hab = homogeneous(eye(3),offset); + AdHab = Adjoint(eye(3),offset); + Hout = Hin * Hab; + Pout.e = transpose(AdHab) * Pin.e; + Pin.f = AdHab * Pout.f; + + implementation_end; + Link3 800 120 + description ' + + 4.8 + Bond Graph\MR\link-v3.emx +1 + False + 2020-7-19 15:22:34 +False + +'; + type 'Submodel-Equation' + ports + signal in Hin [4,4]; + signal out Hout [4,4]; + power in Pin [6,1]; + power out Pout [6,1]; + restrictions + causality constraint not_equal Pin Pout; + end; + icon bg bottom + figures + rectangle 768 96 832 144 color 0 fill 8454041; + text 'Link' 800 120 color 0 'Clear Sans' 16; + terminals + Hin 768 136 fixed; + Hout 832 136 fixed; + Pin 832 104 fixed; + Pout 768 104 fixed; + end; + implementation eq +parameters + real offset[3]= [0;0.025;0]; //coordinates of joint_1 +variables + real Hab[4,4]; + real AdHab[6,6]; +equations + Hab = homogeneous(eye(3),offset); + AdHab = Adjoint(eye(3),offset); + Hout = Hin * Hab; + Pout.e = transpose(AdHab) * Pin.e; + Pin.f = AdHab * Pout.f; + + implementation_end; + motor_joint1 112 384 + description ' + 4.8 + 1 + '; + type Submodel + ports + signal in input; + power out p2; + end; + implementation bg + submodels + Gain1 552 88 + description '4.01False + Signal\Block Diagram\Gain.emx + 2007-9-26 12:15:12 +'; + type Gain + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 536.1 72 567.9 104 color 0 fill 15132390; + text 'K' 552 88 color 16711680 16 bold; + end; + implementation eq +parameters + real K = 12.0; // gain +equations + output = K * input; + implementation_end; + GY 624 184 + description ' + + 4.2 +1 + False + Bond Graph\GY.emx + 2011-11-29 15:53:45 + +'; + type GY + ports + power in p1; + power out p2; + restrictions + causality constraint equal p1 p2; + end; + icon bg bottom + figures + text 'GY' 624 184 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.127; +equations + p1.e = r * p2.f; + p2.e = r * p1.f; +implementation_end; + MSe 624 88 + description ' + + 4.2 +1 + False + Bond Graph\MSe.emx + 2011-11-29 16:12:33 + +'; + type MSe + ports + power out p; + signal in effort; + restrictions + causality fixed out p; + end; + icon bg bottom + figures + text 'MSe' 624 88 color 0 18 bold; + end; + implementation eq +variables + real flow; +equations + p.e = effort; + flow = p.f; +implementation_end; + plug input 424 88; + plug p2 624 220; + OneJunction2 624 136 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 624 136 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + R 664 136 + description ' + + 4.2 +1 + False + Bond Graph\R.emx + 2011-11-29 16:35:37 + +'; + type R + ports + power in p; + end; + icon bg bottom + figures + text 'R' 664 136 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.08; +equations + p.e = r * p.f; +implementation_end; + SignalLimiter2 488 88 + description '4.01False + Signal\Block Diagram Non-Linear\SignalLimiter-Limit.emx + 2007-9-26 12:47:40 +'; + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 472 72 504 104 color 0 fill 15132390; + line 487.9 76.5 487.9 101 color 0 fill 15132390; + line 475 88.2 500.7 88.2 color 0 fill 15132390; + spline 481.9 95.1 493.9 81.4 color 16711680 fill 15132390 width 2; + spline 493.3 82.2 501.9 81.8 color 16711680 fill 15132390 width 2; + spline 475 95.6 481.9 95.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real maximum = 1; + real minimum = -1; +equations + output = limit (input, minimum, maximum); + implementation_end; + end; + connections + Gain1\output -> MSe\effort; + GY\p2 => p2; + input -> SignalLimiter2\input; + MSe\p => OneJunction2\p; + OneJunction2\p => GY\p1; + R\p <= OneJunction2\p; + SignalLimiter2\output -> Gain1\input; + end; + implementation_end; + motor_joint2 680 432 + description ' + 4.8 + 1 + '; + type Submodel + ports + signal in input; + power out p2; + end; + implementation bg + submodels + Gain1 552 88 + description '4.01False + Signal\Block Diagram\Gain.emx + 2007-9-26 12:15:12 +'; + type Gain + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 536.1 72 567.9 104 color 0 fill 15132390; + text 'K' 552 88 color 16711680 16 bold; + end; + implementation eq +parameters + real K = 12.0; // gain +equations + output = K * input; + implementation_end; + GY 624 184 + description ' + + 4.2 +1 + False + Bond Graph\GY.emx + 2011-11-29 15:53:45 + +'; + type GY + ports + power in p1; + power out p2; + restrictions + causality constraint equal p1 p2; + end; + icon bg bottom + figures + text 'GY' 624 184 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.127; +equations + p1.e = r * p2.f; + p2.e = r * p1.f; +implementation_end; + MSe 624 88 + description ' + + 4.2 +1 + False + Bond Graph\MSe.emx + 2011-11-29 16:12:33 + +'; + type MSe + ports + power out p; + signal in effort; + restrictions + causality fixed out p; + end; + icon bg bottom + figures + text 'MSe' 624 88 color 0 18 bold; + end; + implementation eq +variables + real flow; +equations + p.e = effort; + flow = p.f; +implementation_end; + plug input 424 88; + plug p2 624 220; + OneJunction2 624 136 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 624 136 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + R 664 136 + description ' + + 4.2 +1 + False + Bond Graph\R.emx + 2011-11-29 16:35:37 + +'; + type R + ports + power in p; + end; + icon bg bottom + figures + text 'R' 664 136 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.08; +equations + p.e = r * p.f; +implementation_end; + SignalLimiter2 488 88 + description '4.01False + Signal\Block Diagram Non-Linear\SignalLimiter-Limit.emx + 2007-9-26 12:47:40 +'; + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 472 72 504 104 color 0 fill 15132390; + line 487.9 76.5 487.9 101 color 0 fill 15132390; + line 475 88.2 500.7 88.2 color 0 fill 15132390; + spline 481.9 95.1 493.9 81.4 color 16711680 fill 15132390 width 2; + spline 493.3 82.2 501.9 81.8 color 16711680 fill 15132390 width 2; + spline 475 95.6 481.9 95.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real maximum = 1; + real minimum = -1; +equations + output = limit (input, minimum, maximum); + implementation_end; + end; + connections + Gain1\output -> MSe\effort; + GY\p2 => p2; + input -> SignalLimiter2\input; + MSe\p => OneJunction2\p; + OneJunction2\p => GY\p1; + R\p <= OneJunction2\p; + SignalLimiter2\output -> Gain1\input; + end; + implementation_end; + Negate1 272 448 + description '4.01False + Signal\Block Diagram\Negate.emx + 2007-9-26 12:14:11 +'; + type Negate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 256 432 288 464 color 0 fill 15132390; + text '-1' 272 448 color 16711680 16 bold; + end; + implementation eq +equations + output = - input; + implementation_end; + Negate2 520 520 + description '4.01False + Signal\Block Diagram\Negate.emx + 2007-9-26 12:14:11 +'; + type Negate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 504 504 536 536 color 0 fill 15132390; + text '-1' 520 520 color 16711680 16 bold; + end; + implementation eq +equations + output = - input; + implementation_end; + new_joint1 376 256 + description '4.0 + Template\Submodel-Equation.emx +1 + False + 2007-11-1 22:32:1 + False +'; + type 'Submodel-Equation' + ports + signal in input [4,4]; + signal out output; + end; + implementation eq +variables + real position[3]; + real rotation[3]; + real R[3,3]; +equations + position = input[1:3,4]; + rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); + //[input[3,2];input[1,3];input[2,1]]; + R = input[1:3,1:3];implementation_end; + new_joint2 824 216 + description '4.0 + Template\Submodel-Equation.emx +1 + False + 2007-11-1 22:32:1 + False +'; + type 'Submodel-Equation' + ports + signal in input [4,4]; + signal out output; + end; + implementation eq +variables + real position[3]; + real rotation[3]; + real R[3,3]; +equations + // start typing here + position = input[1:3,4]; + rotation = [input[3,2];input[1,3];input[2,1]]; + R = input[1:3,1:3];implementation_end; + new_joint3 1072 144 + description '4.0 + Template\Submodel-Equation.emx +1 + False + 2007-11-1 22:32:1 + False +'; + type 'Submodel-Equation' + ports + signal in input [4,4]; + signal out output; + end; + implementation eq +variables + real position[3]; + real rotation[3]; + real R[3,3]; +equations + position = input[1:3,4]; + rotation = [input[3,2];input[1,3];input[2,1]]; + R = input[1:3,1:3];implementation_end; + OneJunction1 360 104 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 360 104 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction12 864 104 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 864 104 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction13 856 64 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 856 64 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction17 520 288 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 520 288 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction2 120 104 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 120 104 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction3 704 104 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [6,1]; + signal knot out flow [6,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 704 104 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction4 593 232 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [2,1]; + signal knot out flow [2,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 593 232 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction5 257 240 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [2,1]; + signal knot out flow [2,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 257 240 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction6 257 256 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [3,1]; + signal knot out flow [3,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 257 256 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + OneJunction7 593 248 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [3,1]; + signal knot out flow [3,1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 593 248 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + PID1 584 432 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\PD.emx + 2008-1-17 10:49:7 +'; + type PD + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 568 416 600 448 color 0 fill 15132390; + text 'PD' 584.5 432.1 color 16711680 18 bold; + end; + implementation eq +parameters + real kp = 2 {}; // Proportional gain + real tauD = 0.6 {s}; // Derivative time constant: tauD > 0 + real beta = 0.4{}; // Tameness constant: 0 < beta << 1 +variables + real state, rate; +equations + rate = (kp * error - output) / (beta * tauD); + state = int (rate); + output = state + kp * error / beta; + implementation_end; + PID2 200 384 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\PD.emx + 2008-1-17 10:49:7 +'; + type PD + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 184 368 216 400 color 0 fill 15132390; + text 'PD' 200.5 384.1 color 16711680 18 bold; + end; + implementation eq +parameters + real kp = 2 {}; // Proportional gain + real tauD = 0.6 {s}; // Derivative time constant: tauD > 0 + real beta = 0.4{}; // Tameness constant: 0 < beta << 1 +variables + real state, rate; +equations + rate = (kp * error - output) / (beta * tauD); + state = int (rate); + output = state + kp * error / beta; + implementation_end; + PlusMinus4 272 384 + description '4.01False + Signal\Block Diagram\PlusMinus.emx + 2007-9-27 10:15:13 +'; + knot PlusMinus + ports + signal knot duplicatable in plus [1]; + signal knot duplicatable in minus [1]; + signal knot out output [1]; + end; + icon bg ellipse + figures + ellipse 264 376 280 392 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); + implementation_end; + PlusMinus5 520 432 + description '4.01False + Signal\Block Diagram\PlusMinus.emx + 2007-9-27 10:15:13 +'; + knot PlusMinus + ports + signal knot duplicatable in plus [1]; + signal knot duplicatable in minus [1]; + signal knot out output [1]; + end; + icon bg ellipse + figures + ellipse 512 424 528 440 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); + implementation_end; + PowerMux 200 214 + specifications active 'rot_x' + specification 'rot_x' + description ' + + 4.8 + Bond Graph\MR\PowerMux-Rotation.emx + 2020-7-20 14:35:23 +1 + False + True + +'; + type 'PowerMux-Rotation' + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + causality constraint not_equal input_rot2 output; + causality constraint not_equal input_pos3 output; + end; + icon bg bottom + figures + line 176 216 224 216 color 0 width 2; + rectangle 176 208 224 220 color -1; + text '1' 194 211 color 8421504 8; + terminals + input 184 216 fixed; + output 200 216 fixed; + input_rot2 216 216 fixed; + input_pos3 200 216 fixed; + end; + implementation eq +equations + output.e[1] = input.e; + output.e[2:3] = input_rot2.e; + output.e[4:6] = input_pos3.e; + output.f[1] = input.f; + output.f[2:3] = input_rot2.f; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + specification 'rot_y' +description ' + 4.1 + Bond Graph\PowerMux.emx + 2011-3-4 15:12:50 +1 + False + True +'; + type PowerMux + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + end; + icon bg bottom + figures + line 808 216 808 264 color 0 width 2; + rectangle 804 216 816 264 color -1; + text '1' 813 234 color 8421504 8; + terminals + input 808 224 fixed; + output 808 240 fixed; + input_rot2 808 256 fixed; + input_pos3 808 240 fixed; + end; + implementation eq +equations + output.e[1] = input_rot2.e[1]; + output.e[2] = input.e; + output.e[3] = input_rot2.e[2]; + output.e[4:6] = input_pos3.e; + output.f[1] = input_rot2.f[1]; + output.f[2] = input.f; + output.f[3] = input_rot2.f[2]; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + specification 'rot_z' +description ' + 4.1 + Bond Graph\PowerMux.emx + 2011-3-4 15:12:50 +1 + False + True +'; + type PowerMux + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + end; + icon bg bottom + figures + line 808 216 808 264 color 0 width 2; + rectangle 804 216 816 264 color -1; + text '1' 813 234 color 8421504 8; + terminals + input 808 224 fixed; + output 808 240 fixed; + input_rot2 808 256 fixed; + input_pos3 808 240 fixed; + end; + implementation eq +equations + output.e[3] = input.e; + output.e[1:2] = input_rot2.e; + output.e[4:6] = input_pos3.e; + output.f[3] = input.f; + output.f[1:2] = input_rot2.f; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + end; + PowerMux1 536 206 + specifications active 'rot_x' + specification 'rot_x' + description ' + + 4.8 + Bond Graph\MR\PowerMux-Rotation.emx + 2020-7-20 14:35:23 +1 + False + True + +'; + type 'PowerMux-Rotation' + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + causality constraint not_equal input_rot2 output; + causality constraint not_equal input_pos3 output; + end; + icon bg bottom + figures + line 512 208 560 208 color 0 width 2; + rectangle 512 200 560 212 color -1; + text '1' 530 203 color 8421504 8; + terminals + input 520 208 fixed; + output 536 208 fixed; + input_rot2 552 208 fixed; + input_pos3 536 208 fixed; + end; + implementation eq +equations + output.e[1] = input.e; + output.e[2:3] = input_rot2.e; + output.e[4:6] = input_pos3.e; + output.f[1] = input.f; + output.f[2:3] = input_rot2.f; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + specification 'rot_y' +description ' + 4.1 + Bond Graph\PowerMux.emx + 2011-3-4 15:12:50 +1 + False + True +'; + type PowerMux + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + end; + icon bg bottom + figures + line 808 216 808 264 color 0 width 2; + rectangle 804 216 816 264 color -1; + text '1' 813 234 color 8421504 8; + terminals + input 808 224 fixed; + output 808 240 fixed; + input_rot2 808 256 fixed; + input_pos3 808 240 fixed; + end; + implementation eq +equations + output.e[1] = input_rot2.e[1]; + output.e[2] = input.e; + output.e[3] = input_rot2.e[2]; + output.e[4:6] = input_pos3.e; + output.f[1] = input_rot2.f[1]; + output.f[2] = input.f; + output.f[3] = input_rot2.f[2]; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + specification 'rot_z' +description ' + 4.1 + Bond Graph\PowerMux.emx + 2011-3-4 15:12:50 +1 + False + True +'; + type PowerMux + ports + power in input; + power out output [6,1]; + power in input_rot2 [2,1]; + power in input_pos3 [3,1]; + restrictions + causality constraint not_equal input output; + end; + icon bg bottom + figures + line 808 216 808 264 color 0 width 2; + rectangle 804 216 816 264 color -1; + text '1' 813 234 color 8421504 8; + terminals + input 808 224 fixed; + output 808 240 fixed; + input_rot2 808 256 fixed; + input_pos3 808 240 fixed; + end; + implementation eq +equations + output.e[3] = input.e; + output.e[1:2] = input_rot2.e; + output.e[4:6] = input_pos3.e; + output.f[3] = input.f; + output.f[1:2] = input_rot2.f; + output.f[4:6] = input_pos3.f;implementation_end; +specification_end; + end; + R1 472 288 + description ' + + 4.2 +1 + False + Bond Graph\R.emx + 2011-11-29 16:35:37 + +'; + type R + ports + power in p; + end; + icon bg bottom + figures + text 'R' 472 288 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.01; +equations + p.e = r * p.f; +implementation_end; + R2 257 184 + description '4.01False + Bond Graph\2D\R-2.emx + 2007-9-25 12:6:54 +'; + type 'R-2' + ports + power in p [2,1]; + end; + icon bg bottom + figures + text 'R' 257 184 color 0 18 bold; + end; + implementation eq +parameters + real r[2,2] = [1.0, 0.0; 0.0, 1.0] {kN.m.s/rad}; +equations + p.e = r * p.f; +implementation_end; + R3 257 312 + description '4.01False + Bond Graph\3D\R-3.emx + 2007-9-25 12:11:54 +'; + type 'R-3' + ports + power in p [3,1]; + end; + icon bg bottom + figures + text 'R' 257 312 color 0 18 bold; + end; + implementation eq +parameters + real r[3,3] = [1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0] {kN.s/m}; +equations + p.e = r * p.f; +implementation_end; + R4 593 176 + description '4.01False + Bond Graph\2D\R-2.emx + 2007-9-25 12:6:54 +'; + type 'R-2' + ports + power in p [2,1]; + end; + icon bg bottom + figures + text 'R' 593 176 color 0 18 bold; + end; + implementation eq +parameters + real r[2,2] = [1.0, 0.0; 0.0, 1.0] {kN.m.s/rad}; +equations + p.e = r * p.f; +implementation_end; + R5 593 304 + description '4.01False + Bond Graph\3D\R-3.emx + 2007-9-25 12:11:54 +'; + type 'R-3' + ports + power in p [3,1]; + end; + icon bg bottom + figures + text 'R' 593 304 color 0 18 bold; + end; + implementation eq +parameters + real r[3,3] = [1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0] {kN.s/m}; +equations + p.e = r * p.f; +implementation_end; + R6 464 256 + description ' + + 4.2 +1 + False + Bond Graph\R.emx + 2011-11-29 16:35:37 + +'; + type R + ports + power in p; + end; + icon bg bottom + figures + text 'R' 464 256 color 0 18 bold; + end; + implementation eq +parameters + real r = 1000; +equations + p.e = r * p.f; +implementation_end; + R7 56 288 + description ' + + 4.2 +1 + False + Bond Graph\R.emx + 2011-11-29 16:35:37 + +'; + type R + ports + power in p; + end; + icon bg bottom + figures + text 'R' 56 288 color 0 18 bold; + end; + implementation eq +parameters + real r = 0.01; +equations + p.e = r * p.f; +implementation_end; + rectanglepath 80 520 + description ' + + 4.8 + setpoint\rectanglepath.emx +1 + False + 2020-7-10 12:30:38 +False + +'; + type 'Submodel-Equation' + ports + signal out output [2,1]; + end; + implementation eq +/* +This will generate coordinates for the box that has to be drawn +*/ +parameters + real w = 0.07{m} ; + real h = 0.05 {m}; + real t = 0.5 {s}; + real origin[2] = [0.01,0.03]{m}; +variables + real v {m/s}; + real t_w {s}; + real t_h {s}; + real period {s}; + real test1, test2, test3; +initialequations + v = (2*w + 2*h)/t; + t_w = w / v; + t_h = h / v; +equations + period = floor(time / t) * t; + output[1] = v*(ramp(period+t_h) - ramp(period + t_w+t_h) - ramp(period + t_w + t_h + t_h)) + origin[1]; + output[2] = v*(ramp(period) - ramp(period + t_h) - ramp(period + t_h + t_w) + ramp(period + 2 * t_h + t_w))+ origin[2]; +implementation_end; + rectanglepath1 272 632 + description ' + + 4.8 + setpoint\rectanglepath.emx +1 + False + 2020-7-10 12:30:38 +False + +'; + type 'Submodel-Equation' + ports + signal out output [2,1]; + end; + icon bg + figures + rectangle 216 616 328 648 color 0 fill 15132390; + text 'name' 272 632 color 0 'Clear Sans' 16; + end; + implementation eq +/* +This will generate coordinates for the box that has to be drawn +*/ +parameters + real w = 0.07{m} ; + real h = 0.05 {m}; + real t = 2 {s}; + real origin[2] = [0.01,0.03]{m}; +variables + real v {m/s}; + real t_w {s}; + real t_h {s}; + real period {s}; + real y; +initialequations + v = (2*w + 2*h)/t; + t_w = w / v; + t_h = h / v; +code +y = frequencyevent (t,0); +period = time - floor(time / t) * t; + +if period < t_w or time > 3 then + output = origin; +else + if period < t_w + t_h then + output = origin + [w;0]; + else + if period < 2 * t_w + t_h then + output = origin + [w;h]; + else + output = origin + [0;h]; + end; + end; +end; + + + + //output[1] = v*(ramp(period+t_h) - ramp(period + t_w+t_h) - ramp(period + t_w + t_h + t_h)) + origin[1]; + //output[2] = v*(ramp(period) - ramp(period + t_h) - ramp(period + t_h + t_w) + ramp(period + 2 * t_h + t_w))+ origin[2]; +implementation_end; + Sf2 64 104 + description '4.01False + Bond Graph\2D\Sf-2.emx + 2007-9-25 12:7:5 +'; + type 'Sf-2' + ports + power out p [6,1]; + restrictions + causality fixed in p; + end; + icon bg bottom + figures + text 'Sf' 64 104 color 0 18 bold; + end; + implementation eq +parameters + real flow[6,1] = 0; +variables + real effort [6]; +equations + p.f = flow; + effort = p.e; +implementation_end; + Splitter1 392 136 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 388.8 132.8 395.2 139.2 color -1 fill 0; + ellipse 387.7 131.7 396.3 140.3 color -1; + terminals + input 392 136 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Splitter2 736 136 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [4,4]; + signal knot in input [4,4]; + end; + icon bg ellipse + figures + ellipse 732.8 132.8 739.2 139.2 color -1 fill 0; + ellipse 731.7 131.7 740.3 140.3 color -1; + terminals + input 736 136 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Zero 112 136 + description '4.01False + Signal\Sources\Zero.emx + 2007-9-27 15:54:36 + +'; + type Zero + ports + signal out output [4,4]; + end; + icon bg bottom + figures + rectangle 96.1 120 127.9 152 color 0 fill 15132390; + text '0' 112 136 color 16711680 18 bold; + end; + implementation eq +equations + output = eye(4);implementation_end; + ZeroJunction1 112 328 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 112 328 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + ZeroJunction2 656 368 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 656 368 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + ZeroJunction7 112 288 + description ' + + 4.2 +1 + False + Bond Graph\OneJunction.emx + 2011-11-29 16:17:51 + +'; + knot OneJunction + ports + power knot duplicatable none p [1]; + signal knot out flow [1]; + restrictions + causality constraint one_out p; + end; + icon bg + figures + text '1' 112 288 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + end; + connections + Base1\Hout -> new_joint2\input; + Base2\Hout -> new_joint3\input; + Base3\Hin <- Splitter1\output; + Base3\Hout -> new_joint1\input; + C1\p <= OneJunction7\p; + C2\p <= OneJunction6\p; + Integrate\input <- ZeroJunction7\flow; + Integrate\output -> PlusMinus4\minus; + Integrate1\output -> PlusMinus5\minus; + inverse_kinematics1\angle1 -> Negate1\input; + inverse_kinematics1\angle2 -> Negate2\input; + Joint1\Hout -> Link\Hin; + Joint1\Pdiff <= PowerMux\output; + Joint1\Pin <= Link\Pout; + Joint1\Pout => OneJunction2\p; + Joint2\Hout -> Link2\Hin; + Joint2\Pdiff <= PowerMux1\output; + Joint2\Pin <= Link2\Pout; + Link\Pin <= OneJunction1\p; + Link1\Hout -> Joint2\Hin; + Link1\Pin <= Joint2\Pout; + Link1\Pout => OneJunction1\p; + Link2\Hout -> Splitter2\input; + Link2\Pin <= OneJunction3\p; + Link3\Hout -> Base2\Hin; + Link3\Pin <= OneJunction12\p; + Link3\Pout => OneJunction3\p; + motor_joint1\p2 => ZeroJunction1\p; + motor_joint2\p2 => ZeroJunction2\p; + Negate1\output -> PlusMinus4\plus; + Negate2\output -> PlusMinus5\plus; + OneJunction1\p => Base3\p; + OneJunction13\p => Base2\p; + OneJunction17\flow -> Integrate1\input; + OneJunction17\p => C5\p; + OneJunction3\p => Base1\p; + OneJunction4\p => C4\p; + OneJunction4\p => R4\p; + OneJunction5\p => C3\p; + OneJunction5\p => R2\p; + OneJunction6\p => R3\p; + OneJunction7\p => R5\p; + PID1\error <- PlusMinus5\output; + PID1\output -> motor_joint2\input; + PID2\output -> motor_joint1\input; + PlusMinus4\output -> PID2\error; + PowerMux\input_pos3 <= OneJunction6\p; + PowerMux\input_rot2 <= OneJunction5\p; + PowerMux1\input <= OneJunction17\p; + PowerMux1\input_pos3 <= OneJunction7\p; + PowerMux1\input_rot2 <= OneJunction4\p; + R1\p <= OneJunction17\p; + R6\p <= OneJunction17\p; + R7\p <= ZeroJunction7\p; + rectanglepath1\output -> inverse_kinematics1\input; + Sf2\p => OneJunction2\p; + Splitter1\input <- Link\Hout; + Splitter1\output -> Link1\Hin; + Splitter2\output -> Base1\Hin; + Splitter2\output -> Link3\Hin; + Zero\output -> Joint1\Hin; + ZeroJunction7\p => PowerMux\input; + end; + implementation_end; +]]> + + + + + + + Experiment 1 + + + + 4.8 + + + + + + + + + + + + Base1\InertialTensor\state_initial + 6 + 1 + 0 0 0 0 0 0 + + + Base2\InertialTensor\state_initial + 6 + 1 + 0 0 0 0 0 0 + + + Base3\InertialTensor\state_initial + 6 + 1 + 0 0 0 0 0 0 + + + C1\state_initial + 3 + 1 + 0 0 0 + + + C2\state_initial + 3 + 1 + 0 0 0 + + + C3\state_initial + 2 + 1 + 0 0 + + + C4\state_initial + 2 + 1 + 0 0 + + + C5\state_initial + 0 + + + Joint1\Hmatrix\p_initial + 3 + 1 + 0 0 0 + + + Joint2\Hmatrix\p_initial + 3 + 1 + 0 0 0 + + + PID1\state_initial + 0 + + + PID2\state_initial + 0 + + + + + + time + new_joint1\position[1] + new_joint1\position[2] + new_joint1\position[3] + new_joint1\R[1,3] + new_joint1\R[2,3] + new_joint1\R[3,3] + new_joint1\R[1,2] + new_joint1\R[2,2] + new_joint1\R[3,2] + new_joint2\R[1,3] + new_joint2\R[2,3] + new_joint2\R[3,3] + new_joint2\R[1,2] + new_joint2\R[2,2] + new_joint2\R[3,2] + new_joint2\position[1] + new_joint2\position[2] + new_joint2\position[3] + PID2\error + PID3\error + new_joint3\position[3] + new_joint3\position[2] + Submodel3\body_position[1] + Submodel3\body_position[2] + Submodel3\body_position[3] + Submodel3\body_angle[1] + Submodel3\body_angle[2] + Submodel3\body_angle[3] + Submodel3\dimension[1] + Submodel3\dimension[2] + Submodel3\dimension[3] + Submodel4\body_position[1] + Submodel4\body_position[2] + Submodel4\body_position[3] + Submodel4\body_angle[1] + Submodel4\body_angle[2] + Submodel4\body_angle[3] + Submodel4\dimension[1] + Submodel4\dimension[2] + Submodel4\dimension[3] + rectanglepath1\output[2] + rectanglepath1\output[1] + Step\output + Integrate1\output + motor_joint2\SignalLimiter2\output + + + + D3DPlot + + 1 + false + 16777215 + true + + 3D Animation + 137 + true + false + 4294967295 + Gradients\BlueWhite.png + true + 1.0 + 1.0 + + + + + 1 + + + 1 + + + 1 + + + true + Reference Frame + + + + + + Bryant + + false + false + false + false + false + false + + + + + 1 + + + 1 + + + 1 + + + false + Default Lights and Cameras + + + + + + Bryant + + false + false + false + false + false + false + + + false + Ambient + + + + + + 1 + + + + + 1 + + + Direct3D + + false + false + false + false + false + false + + + 0 + + + + 0.3 + + + 0.3 + + + 0.3 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + + 0.3 + + + 0.3 + + + 0.3 + + + + + + + 1 + + + 1 + + + 1 + + + true + + true + false + false + + + false + Parallel + + + -3 + + + 5 + + + 3 + + + + + + 0.457495710997814 + + + -0.762492851663023 + + + -0.457495710997814 + + + + + 0.235379601434674 + + + -0.392299335724456 + + + 0.889211827642101 + + + Direct3D + + false + false + false + false + false + false + + + 3 + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + 1.0 + + + 0.0 + + + 0.0 + + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + false + true + true + + + false + Spot Light 1 + + + -3 + + + -5 + + + 1 + + + + + + 0.50709255283711 + + + 0.845154254728517 + + + -0.169030850945703 + + + + + 0.0869656553478673 + + + 0.144942758913112 + + + 0.985610760609162 + + + Direct3D + + false + false + false + false + false + false + + + 2 + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + 1.0 + + + 0.05 + + + 0.05 + + + + 1.0471975511965976 + + + 1.0471975511965976 + + 0.0 + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + false + true + true + + + false + Spot Light 2 + + + 2 + + + -3 + + + -1 + + + + + + -0.534522483824849 + + + 0.801783725737273 + + + 0.267261241912424 + + + + + 0.14824986333222 + + + -0.22237479499833 + + + 0.963624111659432 + + + Direct3D + + false + false + false + false + false + false + + + 2 + + + + 1 + + + 1 + + + 1 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + 1.0 + + + 0.05 + + + 0.05 + + + + 1.5707963267948966 + + + 1.5707963267948966 + + 0.0 + + + + 1 + + + 1 + + + 1 + + + + + + + 1 + + + 1 + + + 1 + + + true + + + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + + 1 + + + 1 + + + 1 + + + true + + false + true + true + + + false + Camera Looking at Origin + + + 3.61094505336573 + + + 0.23725177442673 + + + 0.233324732826921 + + + + + + -0.995780766056347 + + + -0.065426294279019 + + + -0.0643433444044585 + + + + + -0.0642049089065914 + + + -0.00421848805226125 + + + 0.997927820050553 + + + Direct3D + + false + false + false + false + false + false + + + 0.01 + 100.0 + true + 0.003926990816987242 + 45.0 + -10.0 + 10.0 + 10.0 + -10.0 + true + + + -9.99200722162641e-016 + + + 1.76247905159244e-015 + + + -3.05678199886544e-015 + + + true + + + 1 + + + true + true + true + + + false + Front(XY)-Camera + + + 10 + + + + + + -1 + + + + + 1 + + + Direct3D + + false + false + false + false + false + false + + + 0.01 + 100.0 + true + 9.999999999999998 + 45.0 + -9.999999999999998 + 9.999999999999998 + 9.999999999999998 + -9.999999999999998 + true + + + true + + + 1 + + + true + false + false + + + false + Side(YZ)-Camera + + + 10 + + + + + + -1 + + + + + 1 + + + Direct3D + + false + false + false + false + false + false + + + 0.01 + 100.0 + true + 9.999999999999998 + 45.0 + -9.999999999999998 + 9.999999999999998 + 9.999999999999998 + -9.999999999999998 + true + + + true + + + 1 + + + true + false + false + + + false + Top(XZ)-Camera + + + 10 + + + + + + -1 + + + + + 1 + + + Direct3D + + false + false + false + false + false + false + + + 0.01 + 100.0 + true + 9.999999999999998 + 45.0 + -9.999999999999998 + 9.999999999999998 + 9.999999999999998 + -9.999999999999998 + true + + + true + + + 1 + + + true + false + false + + + + + + + 1 + + + 1 + + + 1 + + + false + Scenery + + + + + + Bryant + + false + false + false + false + false + false + + + + + 10 + + + 10 + + + 10 + + + false + Reference Frame + + + + + + Bryant + + false + false + false + false + false + false + + +
Center
+ 1.0 + true + 4 + + + Submodel3\dimension[1] + 0.005 + + + Submodel3\dimension[2] + 0.065 + + + Submodel3\dimension[3] + 0.01 + + + true + + + 1 + + + 1 + + + 1 + + + 1.0 + + + 1 + + + 1 + + + 1 + + + + + true + + + 0.498039215686275 + + + 0.498039215686275 + + + 0.498039215686275 + + + + + + 14.298713684082 + + false + false + Block + + + Submodel3\body_position[1] + 0.0 + + + Submodel3\body_position[2] + -0.009158353632383561 + + + Submodel3\body_position[3] + 0.03118053280824738 + + + + + + Submodel3\body_angle[1] + 1.8564889888314844 + + + Submodel3\body_angle[2] + 0.0 + + + Submodel3\body_angle[3] + 0.0 + + + Euler + + false + false + false + false + false + false + + + + +
Center
+ 1.0 + true + 4 + + + Submodel4\dimension[1] + 0.005 + + + Submodel4\dimension[2] + 0.05 + + + Submodel4\dimension[3] + 0.01 + + + true + + + 1 + + + 1 + + + 1 + + + 1.0 + + + 1 + + + 1 + + + 1 + + + + + true + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + false + false + Block + + + Submodel4\body_position[1] + 0.0 + + + Submodel4\body_position[2] + 0.0013903741642251554 + + + Submodel4\body_position[3] + 0.07774345829579168 + + + + + + Submodel4\body_angle[1] + 0.66277054304191 + + + Submodel4\body_angle[2] + 0.0 + + + Submodel4\body_angle[3] + 0.0 + + + Euler + + false + false + false + false + false + false + + + + + + + + + 10 + + + 10 + + + 10 + + + false + Reference Frame + + + 0.5 + + + + + + Bryant + + false + false + false + false + false + false + + +
Center
+ 1.0 + true + 4 + + + Submodel3\dimension[1] + 0.005 + + + Submodel3\dimension[2] + 0.065 + + + Submodel3\dimension[3] + 0.01 + + + true + + + 1 + + + 1 + + + 1 + + + 1.0 + + + 1 + + + 1 + + + 1 + + + + + true + + + 0.498039215686275 + + + 0.498039215686275 + + + 0.498039215686275 + + + + + + 14.298713684082 + + false + false + Block + + + new_joint1\position[1] + 0.0 + + + new_joint1\position[2] + 0.02758023578749541 + + + new_joint1\position[3] + -0.017200292914302247 + + + + + + new_joint1\R[1,3] + 0.0 + + + new_joint1\R[2,3] + 0.5289972876475422 + + + new_joint1\R[3,3] + 0.8486235146762925 + + + + + new_joint1\R[1,2] + 0.0 + + + new_joint1\R[2,2] + 0.8486235146762925 + + + new_joint1\R[3,2] + -0.5289972876475422 + + + Matrix + + false + false + false + false + false + false + + + + +
Center
+ 1.0 + true + 4 + + + Submodel4\dimension[1] + 0.005 + + + Submodel4\dimension[2] + 0.05 + + + Submodel4\dimension[3] + 0.01 + + + true + + + 1 + + + 1 + + + 1 + + + 1.0 + + + 1 + + + 1 + + + 1 + + + + + true + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + false + false + Block + + + new_joint2\position[1] + 0.0 + + + new_joint2\position[2] + 0.07637589665407551 + + + new_joint2\position[3] + -0.04762103432818519 + + + + + + new_joint2\R[1,3] + 0.0 + + + new_joint2\R[2,3] + 0.5289973509267839 + + + new_joint2\R[3,3] + 0.8486234752305908 + + + + + new_joint2\R[1,2] + 0.0 + + + new_joint2\R[2,2] + 0.8486234752305908 + + + new_joint2\R[3,2] + -0.5289973509267839 + + + Matrix + + false + false + false + false + false + false + + + + + + + + + + + + + GraphPlot + + 2 + false + 16777215 + true + + + true + 15780518 + 12624260 + 0 + 10 + 10 + 10 + false + + 16777215 + true + 1 + model(2) + true + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 10 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + true + true + false + + + -0.04000000000000001 + 0.15999999999999998 + true + 1 + + + + + + -0.1 + 0.1 + true + 1 + + + + -0.06 + 0.14 + true + 1 + + + + + + + + 3355111 + 1 + 3355111 + 0 + true + 1 + 1 + 1 + true + + true + new_joint3\position[2] + + + true + new_joint3\position[3] + + + + 6076255 + 1 + 6076255 + 0 + true + 1 + 1 + 1 + true + + true + rectanglepath1\output[1] + + + true + rectanglepath1\output[2] + + + + + true + 0 + 16777215 + + + + GraphPlot + + 3 + false + 16777215 + true + + + true + 15780518 + 12624260 + 0 + 10 + 10 + 10 + false + + 16777215 + true + 1 + Plot + true + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 10 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + true + true + false + + + 0.0 + 1.3 + true + 3 + + + + + + -2.8 + -0.8000000000000003 + true + 2 + + + + -1885.20460252739 + 0.0 + true + 2 + + + + + + + + 3355111 + 1 + 3355111 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + PID2\error + + + + 6076255 + 1 + 6076255 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + PID3\error + + + + + true + 0 + 16777215 + + + + GraphPlot + + 4 + false + 16777215 + true + + + true + 15780518 + 12624260 + 0 + 10 + 10 + 10 + false + + 16777215 + true + 1 + model + true + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 10 + 34 + 400 + 0 + 0 + 0 + 0 + + + Arial + 12 + 34 + 400 + 0 + 0 + 0 + 0 + + + true + true + false + + + 0.8 + 1.5 + true + 0 + + + + + + -2.5 + 2.5 + true + 1 + + + + -2.5 + 2.5 + true + 1 + + + + -2.5 + 2.5 + true + 1 + + + + + + + + 3355111 + 1 + 3355111 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + Step\output + + + + 6076255 + 1 + 6076255 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + Integrate1\output + -1.0 + + + + 12553035 + 1 + 12553035 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + motor_joint2\SignalLimiter2\output + + + + + true + + 0.9365808823529411 + 0.0811764705882353 + + 0 + 16777215 + + + + + + 1 + true + Window 1 + 0 + + 2 + 4 + + Base + + + 2 + false + Window 2 + 0 + + 1 + + Base + + + 3 + false + Window 3 + 0 + + 3 + + Base + + + + +0.0557292 0.189815 0.747917 0.775 + + +0.0807292 0.133333 0.720313 0.826852 + + +0.286979 0.255556 0.683333 0.693519 + + + + + + 0.0 + 0.1 + false + false + false + false + 0.1 + 1.0e-6 + 1.0e-7 + false + true + + + + Euler + 0.01 + false + + + BackwardEuler + 1.0e-5 + 1.0e-5 + 1.0e-5 + 1.0e-5 + 0.01 + 1.0 + + + AdamsBashforth + 0.01 + false + + + RungeKutta2 + 0.01 + false + + + RungeKutta4 + 0.01 + false + + + RungeKutta8 + false + 0.0 + false + 0.0 + 1.0e-6 + 1.0e-6 + 0.9 + 0.33 + 6.0 + 0.0 + false + 100000 + false + 1000 + + + RungeKuttaFehlberg + false + 0.0 + false + 0.0 + 1.0e-6 + 1.0e-6 + + + VodeAdams + false + 0.0 + false + 0.0 + 1.0e-6 + 1.0e-6 + true + true + + + BDFMethod + 1.0e-5 + 1.0e-5 + 1.0e-5 + 1.0e-5 + false + 0.0 + false + 0.0 + + + MeBDFiMethod + 1.0e-5 + 1.0e-5 + 1.0e-5 + 1.0e-5 + false + 0.0 + false + 0.0 + + 8 + + + + 10 + false + true + true + false + true + 0 + 0.0 + true + MultipleRun + true + + UseEndValue + 0.001 + BroydonFletcherGoldfarbShanno + + + + + true + true + true + true + + + false + + + + + + + + + 1.0 + + + + +