diff --git a/implementation/SCARA/04_path_planning/path_planning.emx b/implementation/SCARA/04_path_planning/path_planning.emx
index ef4617d..3b833f9 100644
--- a/implementation/SCARA/04_path_planning/path_planning.emx
+++ b/implementation/SCARA/04_path_planning/path_planning.emx
@@ -8,7 +8,7 @@
0
False
C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\04_path_planning\path_planning.emx
- 2020-7-20 17:47:38
+ 2020-7-21 13:21:44
True
@@ -31,25 +31,115 @@ initialequations
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};';
+ 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;
+ COM 376 208
+ description '4.81Bond Graph\MR\center_of_mass_v2.emx2020-7-21 12:24:38parameters
+ 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];
+ power out p [6,1];
end;
- icon bg ellipse bottom
+ icon bg 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;
+ rectangle 344 192 408 224 color 0 fill 139;
+ text 'COM' 376 208 color 0 'Clear Sans' 16;
terminals
- p 704 160 fixed;
- Hin 736 160 fixed;
- Hout 744 216 fixed;
+ Hin 392 192 fixed;
+ p 360 192 fixed;
end;
implementation bg
submodels
@@ -108,7 +198,7 @@ equations
Q = transpose(adjoint(p1.f));
QI = Q*II;
p1.e = QI*p1.f;implementation_end;
- Gravity 664 400
+ Gravity 624 400
description '4.01False2007-9-25 12:3:26True';
type Se
ports
@@ -118,7 +208,7 @@ equations
end;
icon bg bottom
figures
- text 'Se' 664 400 color 0 18 bold;
+ text 'Se' 624 400 color 0 18 bold;
end;
implementation eq
parameters
@@ -129,18 +219,21 @@ 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
+ InertialTensor 544 272
+ description '4.01False
+ Bond Graph\3D\I-3.emx
+ 2007-9-25 12:12:14
+';
+ type 'I-3'
ports
power in p [6,1];
signal out state [6,1];
restrictions
causality preferred in p;
end;
- icon bg top
+ icon bg bottom
figures
- text 'I' 544 288 color 0 18 bold;
+ text 'I' 544 272 color 0 18 bold;
end;
implementation eq
parameters
@@ -157,9 +250,9 @@ 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;
+ plug Hin 433.4 496;
+ plug Hout 691.2 496;
+ plug p 439.4 336;
Splitter1 544 496
description '4.0
Signal\Block Diagram\Splitter.emx
@@ -242,50 +335,31 @@ implementation_end;
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;
+ Ta0j\p => InertialTensor\p;
+ ZeroJunction1\p => p;
+ ZeroJunction1\p => Ta0j\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};';
+ COM1 720 200
+ description '4.81Bond Graph\MR\center_of_mass_v2.emx2020-7-21 12:24:38parameters
+ 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];
+ power out p [6,1];
end;
- icon bg ellipse bottom
+ icon bg 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;
+ rectangle 688 184 752 216 color 0 fill 139;
+ text 'COM' 720 200 color 0 'Clear Sans' 16;
terminals
- p 912 104 fixed;
- Hin 912 136 fixed;
- Hout 968 144 fixed;
+ Hin 736 184 fixed;
+ p 704 184 fixed;
end;
implementation bg
submodels
@@ -344,7 +418,7 @@ equations
Q = transpose(adjoint(p1.f));
QI = Q*II;
p1.e = QI*p1.f;implementation_end;
- Gravity 616 400
+ Gravity 624 400
description '4.01False2007-9-25 12:3:26True';
type Se
ports
@@ -354,7 +428,7 @@ equations
end;
icon bg bottom
figures
- text 'Se' 616 400 color 0 18 bold;
+ text 'Se' 624 400 color 0 18 bold;
end;
implementation eq
parameters
@@ -365,18 +439,21 @@ 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
+ InertialTensor 544 272
+ description '4.01False
+ Bond Graph\3D\I-3.emx
+ 2007-9-25 12:12:14
+';
+ type 'I-3'
ports
power in p [6,1];
signal out state [6,1];
restrictions
causality preferred in p;
end;
- icon bg top
+ icon bg bottom
figures
- text 'I' 544 288 color 0 18 bold;
+ text 'I' 544 272 color 0 18 bold;
end;
implementation eq
parameters
@@ -393,9 +470,9 @@ 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;
+ plug Hin 433.4 496;
+ plug Hout 691.2 496;
+ plug p 439.4 336;
Splitter1 544 496
description '4.0
Signal\Block Diagram\Splitter.emx
@@ -445,67 +522,64 @@ 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 => Ta0j\p;
Splitter1\output -> AdHi0\H;
Splitter1\output -> Hout;
Ta0j\p => EJS\p1;
+ Ta0j\p => InertialTensor\p;
+ ZeroJunction1\p => p;
+ ZeroJunction1\p => Ta0j\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;
- 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};
-
-';
+ COM2 904 184
+ description '4.81Bond Graph\MR\center_of_mass_v2.emx2020-7-21 12:24:38parameters
+ real I [3,1] = [0.018;0.159;0.159] {mN.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];
+ power out p [6,1];
end;
- icon bg ellipse bottom
+ icon bg 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;
+ rectangle 872 168 936 200 color 0 fill 139;
+ text 'COM' 904 184 color 0 'Clear Sans' 16;
terminals
- p 360 160 fixed;
- Hin 392 160 fixed;
+ Hin 920 168 fixed;
+ p 888 168 fixed;
end;
implementation bg
submodels
@@ -564,7 +638,7 @@ equations
Q = transpose(adjoint(p1.f));
QI = Q*II;
p1.e = QI*p1.f;implementation_end;
- Gravity 656 400
+ Gravity 624 400
description '4.01False2007-9-25 12:3:26True';
type Se
ports
@@ -574,7 +648,7 @@ equations
end;
icon bg bottom
figures
- text 'Se' 656 400 color 0 18 bold;
+ text 'Se' 624 400 color 0 18 bold;
end;
implementation eq
parameters
@@ -585,18 +659,21 @@ 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
+ InertialTensor 544 272
+ description '4.01False
+ Bond Graph\3D\I-3.emx
+ 2007-9-25 12:12:14
+';
+ type 'I-3'
ports
power in p [6,1];
signal out state [6,1];
restrictions
causality preferred in p;
end;
- icon bg top
+ icon bg bottom
figures
- text 'I' 544 288 color 0 18 bold;
+ text 'I' 544 272 color 0 18 bold;
end;
implementation eq
parameters
@@ -613,9 +690,9 @@ 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;
+ plug Hin 433.4 496;
+ plug Hout 691.2 496;
+ plug p 424 336;
Splitter1 544 496
description '4.0
Signal\Block Diagram\Splitter.emx
@@ -666,7 +743,7 @@ equations
equal (collect (p.f));
flow = first (p.f);
implementation_end;
- ZeroJunction1 504 336
+ ZeroJunction1 480 336
description '
4.2
@@ -685,7 +762,7 @@ implementation_end;
end;
icon bg
figures
- text '0' 504 336 color 0 18 bold;
+ text '0' 480 336 color 0 18 bold;
end;
implementation eq
equations
@@ -698,151 +775,14 @@ implementation_end;
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;
+ Ta0j\p => InertialTensor\p;
+ ZeroJunction1\p => p;
+ ZeroJunction1\p => Ta0j\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 '
@@ -925,7 +865,6 @@ parameters
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];
@@ -941,32 +880,33 @@ equations
implementation_end;
- Joint1 200 120
+ Joint 192 120
description '
4.8
1
Bond Graph\MR\joint-v3.emx
- 2020-7-20 14:25:50
+ 2020-7-21 12:08:53
';
type 'Submodel-v3'
ports
power in Pin [6,1];
- signal in Hin [4,4];
power in Pdiff [6,1];
+ signal in Hin [4,4];
signal out Hout [4,4];
+ signal out Hdiff [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;
+ rectangle 168 88 216 152 color 0 fill 14745599;
+ text 'Joint' 192 120 color 0 'Clear Sans' 16;
terminals
- Pin 224 104 fixed;
- Hin 176 136 fixed;
- Hout 224 136 fixed;
- Pout 176 104 fixed;
+ Pin 216 104 fixed;
+ Hin 168 136 fixed;
+ Hout 216 136 fixed;
+ Pout 168 104 fixed;
end;
implementation bg
submodels
@@ -995,7 +935,7 @@ equations
equations
p2.e = transpose(Adjoint(H)) * p1.e;
p1.f = Adjoint(H) * p2.f;implementation_end;
- FlowSensor2 240 271.9
+ FlowSensor2 184 311.9
description '
4.2
@@ -1015,8 +955,8 @@ equations
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;
+ ellipse 177.1 304.8 190.9 319.1 color 0 fill 16777215;
+ text 'f' 184 311.2 color 0;
end;
implementation eq
equations
@@ -1024,13 +964,18 @@ equations
p1.e = p2.e;
flow = p1.f;
implementation_end;
- Hmatrix 320 272
+ 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;
+ icon bg
+ figures
+ rectangle 224 296 288 328 color 0 fill 15132390;
+ text 'name' 256 312 color 0 'Clear Sans' 16;
+ end;
implementation eq
parameters
real init[4] = [1;0;0;0];
@@ -1040,13 +985,17 @@ variables
real R[3,3]; //Rotation Matrix
real p[3]; //Position Vector
real dq[4];
+ real Wb[3,4];
equations
- dq = transpose(W) * flow[1:3] ./ 2;
+ dq = transpose(Wb) * 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]];
+ Wb = [ -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];
@@ -1075,11 +1024,12 @@ equations
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;
+ plug Pin 492.1 424;
+ plug Pdiff 184 225;
+ plug Hin 130.8 576;
+ plug Hout 478.4 576;
+ plug Hdiff 320 222;
+ plug Pout 134.7 424;
Splitter2 320 312
description '4.0
Signal\Block Diagram\Splitter.emx
@@ -1103,7 +1053,7 @@ equations
equations
collect (output) = input;
implementation_end;
- Wbai 240 424
+ Wbai 184 424
description '
4.0
1
@@ -1121,7 +1071,7 @@ implementation_end;
end;
icon bg bottom
figures
- text '0' 240 424 color 0 18 bold;
+ text '0' 184 424 color 0 18 bold;
end;
implementation eq
equations
@@ -1139,43 +1089,29 @@ implementation_end;
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;
- Joint2 536 120
+ Joint1 536 120
description '
4.8
1
Bond Graph\MR\joint-v3.emx
- 2020-7-20 14:25:50
+ 2020-7-21 12:08:53
';
type 'Submodel-v3'
ports
power in Pin [6,1];
- signal in Hin [4,4];
power in Pdiff [6,1];
+ signal in Hin [4,4];
signal out Hout [4,4];
- power out Pout [6,1];
signal out Hdiff [4,4];
+ power out Pout [6,1];
end;
icon bg bottom
figures
@@ -1250,6 +1186,11 @@ implementation_end;
signal in flow [6,1];
signal out H [4,4];
end;
+ icon bg
+ figures
+ rectangle 224 296 288 328 color 0 fill 15132390;
+ text 'name' 256 312 color 0 'Clear Sans' 16;
+ end;
implementation eq
parameters
real init[4] = [1;0;0;0];
@@ -1258,18 +1199,22 @@ variables
real W[3,4]; //Quaternion Rates Matrix
real R[3,3]; //Rotation Matrix
real p[3]; //Position Vector
+ real dq[4];
+ real Wb[3,4];
equations
- ddt(q,init) = transpose(W) * flow[1:3] ./ 2;
+ dq = transpose(Wb) * 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 = dll('EulerAngles.dll','RotationMatrixFromQuaternion',q);
+ Wb = [ -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 '
@@ -1294,12 +1239,12 @@ equations
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;
+ plug Pin 492.1 424;
+ plug Pdiff 184 225;
+ plug Hin 130.8 576;
+ plug Hout 478.4 576;
+ plug Hdiff 320 222;
+ plug Pout 134.7 424;
Splitter2 320 312
description '4.0
Signal\Block Diagram\Splitter.emx
@@ -1364,21 +1309,6 @@ implementation_end;
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 '
@@ -1387,7 +1317,7 @@ parameterrelations_end;
Bond Graph\MR\link-v3.emx
1
False
- 2020-7-19 15:22:34
+ 2020-7-21 11:14:41
False
';
@@ -1412,13 +1342,18 @@ parameterrelations_end;
end;
implementation eq
parameters
- real offset[3]= [0;0.0325;0]; //coordinates of joint_1
+ 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
- Hab = homogeneous(eye(3),offset);
- AdHab = Adjoint(eye(3),offset);
Hout = Hin * Hab;
Pout.e = transpose(AdHab) * Pin.e;
Pin.f = AdHab * Pout.f;
@@ -1431,7 +1366,7 @@ equations
Bond Graph\MR\link-v3.emx
1
False
- 2020-7-19 15:22:34
+ 2020-7-21 11:14:41
False
';
@@ -1464,7 +1399,7 @@ variables
real omega[3];
initialequations
omega = offset[1:3];
- R = dll('EulerAngles.dll','RotationMatrixFromEulXYZs',omega);
+ R = dll('EulerAngles.dll','RotationMatrixFromEulXYZr',omega);
Hab = homogeneous(R,offset[4:6]);
AdHab = Adjoint(Hab);
equations
@@ -1594,7 +1529,7 @@ parameters
equations
output = K * input;
implementation_end;
- GY 624 184
+ GY 624 208
description '
4.2
@@ -1613,7 +1548,7 @@ equations
end;
icon bg bottom
figures
- text 'GY' 624 184 color 0 18 bold;
+ text 'GY' 624 208 color 0 18 bold;
end;
implementation eq
parameters
@@ -1651,7 +1586,7 @@ equations
flow = p.f;
implementation_end;
plug input 424 88;
- plug p2 624 220;
+ plug p2 624 296;
OneJunction2 624 136
description '
@@ -1679,7 +1614,7 @@ equations
equal (collect (p.f));
flow = first (p.f);
implementation_end;
- R 664 136
+ R 680 136
description '
4.2
@@ -1695,11 +1630,11 @@ implementation_end;
end;
icon bg bottom
figures
- text 'R' 664 136 color 0 18 bold;
+ text 'R' 680 136 color 0 18 bold;
end;
implementation eq
parameters
- real r = 0.08;
+ real r = 2;
equations
p.e = r * p.f;
implementation_end;
@@ -1775,7 +1710,7 @@ parameters
equations
output = K * input;
implementation_end;
- GY 624 184
+ GY 624 200
description '
4.2
@@ -1794,7 +1729,7 @@ equations
end;
icon bg bottom
figures
- text 'GY' 624 184 color 0 18 bold;
+ text 'GY' 624 200 color 0 18 bold;
end;
implementation eq
parameters
@@ -1832,7 +1767,7 @@ equations
flow = p.f;
implementation_end;
plug input 424 88;
- plug p2 624 220;
+ plug p2 624 256;
OneJunction2 624 136
description '
@@ -1880,7 +1815,7 @@ implementation_end;
end;
implementation eq
parameters
- real r = 0.08;
+ real r = 2;
equations
p.e = r * p.f;
implementation_end;
@@ -1942,7 +1877,7 @@ equations
equations
output = - input;
implementation_end;
- Negate2 520 520
+ Negate2 520 552
description '4.01False
Signal\Block Diagram\Negate.emx
2007-9-26 12:14:11
@@ -1954,14 +1889,14 @@ equations
end;
icon bg bottom
figures
- rectangle 504 504 536 536 color 0 fill 15132390;
- text '-1' 520 520 color 16711680 16 bold;
+ rectangle 504 536 536 568 color 0 fill 15132390;
+ text '-1' 520 552 color 16711680 16 bold;
end;
implementation eq
equations
output = - input;
implementation_end;
- new_joint1 376 256
+ new_joint1 376 280
description '4.0
Template\Submodel-Equation.emx
1
@@ -1972,7 +1907,6 @@ equations
type 'Submodel-Equation'
ports
signal in input [4,4];
- signal out output;
end;
implementation eq
variables
@@ -1984,7 +1918,7 @@ equations
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
+ new_joint2 720 256
description '4.0
Template\Submodel-Equation.emx
1
@@ -1995,7 +1929,6 @@ equations
type 'Submodel-Equation'
ports
signal in input [4,4];
- signal out output;
end;
implementation eq
variables
@@ -2007,7 +1940,7 @@ equations
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
+ new_joint3 1016 184
description '4.0
Template\Submodel-Equation.emx
1
@@ -2018,7 +1951,6 @@ equations
type 'Submodel-Equation'
ports
signal in input [4,4];
- signal out output;
end;
implementation eq
variables
@@ -2056,34 +1988,7 @@ equations
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
+ OneJunction12 888 104
description '
4.2
@@ -2102,7 +2007,7 @@ implementation_end;
end;
icon bg
figures
- text '1' 856 64 color 0 18 bold;
+ text '1' 888 104 color 0 18 bold;
end;
implementation eq
equations
@@ -2321,7 +2226,7 @@ implementation_end;
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
+ real beta = 0.2{}; // Tameness constant: 0 < beta << 1
variables
real state, rate;
equations
@@ -2351,7 +2256,7 @@ equations
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
+ real beta = 0.2{}; // Tameness constant: 0 < beta << 1
variables
real state, rate;
equations
@@ -2397,7 +2302,7 @@ equations
equations
output = sum (collect (plus)) - sum (collect (minus));
implementation_end;
- PowerMux 200 214
+ PowerMux 192 214
specifications active 'rot_x'
specification 'rot_x'
description '
@@ -2423,14 +2328,14 @@ equations
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;
+ line 168 216 216 216 color 0 width 2;
+ rectangle 168 208 216 220 color -1;
+ text '1' 186 211 color 8421504 8;
terminals
- input 184 216 fixed;
- output 200 216 fixed;
- input_rot2 216 216 fixed;
- input_pos3 200 216 fixed;
+ input 176 216 fixed;
+ output 192 216 fixed;
+ input_rot2 208 216 fixed;
+ input_pos3 192 216 fixed;
end;
implementation eq
equations
@@ -2663,7 +2568,7 @@ specification_end;
end;
implementation eq
parameters
- real r = 0.01;
+ real r = 0.0001;
equations
p.e = r * p.f;
implementation_end;
@@ -2742,30 +2647,6 @@ 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 '
@@ -2787,11 +2668,11 @@ implementation_end;
end;
implementation eq
parameters
- real r = 0.01;
+ real r = 0.0001;
equations
p.e = r * p.f;
implementation_end;
- rectanglepath 80 520
+ rectanglepath 104 552
description '
4.8
@@ -2820,7 +2701,6 @@ variables
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;
@@ -2830,7 +2710,7 @@ equations
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
+ rectanglepath1 272 656
description '
4.8
@@ -2847,8 +2727,8 @@ implementation_end;
end;
icon bg
figures
- rectangle 216 616 328 648 color 0 fill 15132390;
- text 'name' 272 632 color 0 'Clear Sans' 16;
+ rectangle 216 640 328 672 color 0 fill 15132390;
+ text 'name' 272 656 color 0 'Clear Sans' 16;
end;
implementation eq
/*
@@ -2857,14 +2737,14 @@ 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 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 y;
+ boolean y;
initialequations
v = (2*w + 2*h)/t;
t_w = w / v;
@@ -2873,7 +2753,7 @@ code
y = frequencyevent (t,0);
period = time - floor(time / t) * t;
-if period < t_w or time > 3 then
+if period < t_w or time < 2 then
output = origin;
else
if period < t_w + t_h then
@@ -3063,42 +2943,39 @@ equations
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;
+ COM\Hout -> new_joint1\input;
+ COM1\Hout -> new_joint2\input;
+ COM2\Hin <- Link3\Hout 920 136;
+ COM2\Hout -> new_joint3\input;
+ COM2\p => OneJunction12\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;
+ Joint\Hout -> Link\Hin;
+ Joint\Pdiff <= PowerMux\output;
+ Joint\Pin <= Link\Pout;
+ Joint\Pout => OneJunction2\p;
Link\Pin <= OneJunction1\p;
- Link1\Hout -> Joint2\Hin;
- Link1\Pin <= Joint2\Pout;
+ Link1\Hout -> Joint1\Hin;
+ Link1\Pin <= Joint1\Pout;
Link1\Pout => OneJunction1\p;
+ Link2\Hin <- Joint1\Hout;
Link2\Hout -> Splitter2\input;
Link2\Pin <= OneJunction3\p;
- Link3\Hout -> Base2\Hin;
+ Link2\Pout => Joint1\Pin;
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;
+ OneJunction1\p <= COM\p;
OneJunction17\flow -> Integrate1\input;
- OneJunction17\p => C5\p;
- OneJunction3\p => Base1\p;
+ OneJunction3\p <= COM1\p;
OneJunction4\p => C4\p;
OneJunction4\p => R4\p;
OneJunction5\p => C3\p;
@@ -3114,16 +2991,19 @@ implementation_end;
PowerMux1\input <= OneJunction17\p;
PowerMux1\input_pos3 <= OneJunction7\p;
PowerMux1\input_rot2 <= OneJunction4\p;
+ PowerMux1\output => Joint1\Pdiff;
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 -> COM\Hin;
Splitter1\output -> Link1\Hin;
- Splitter2\output -> Base1\Hin;
+ Splitter2\output -> COM1\Hin;
Splitter2\output -> Link3\Hin;
- Zero\output -> Joint1\Hin;
+ Zero\output -> Joint\Hin;
+ ZeroJunction1\p => ZeroJunction7\p;
+ ZeroJunction2\p => OneJunction17\p;
ZeroJunction7\p => PowerMux\input;
end;
implementation_end;
@@ -3141,32 +3021,13 @@ implementation_end;
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
@@ -3192,8 +3053,16 @@ implementation_end;
0 0
- C5\state_initial
- 0
+ COM1\InertialTensor\state_initial
+ 6
+ 1
+ 0 0 0 0 0 0
+
+
+ COM\InertialTensor\state_initial
+ 6
+ 1
+ 0 0 0 0 0 0
Joint1\Hmatrix\p_initial
@@ -3202,7 +3071,7 @@ implementation_end;
0 0 0
- Joint2\Hmatrix\p_initial
+ Joint\Hmatrix\p_initial
3
1
0 0 0
@@ -3215,6 +3084,12 @@ implementation_end;
PID2\state_initial
0
+
+ COM2\InertialTensor\p.e_initial
+ 6
+ 1
+ 0 0 0 0 0 0
+
@@ -3238,33 +3113,21 @@ implementation_end;
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]
+ rectanglepath1\output[2]
+ rectanglepath1\output[1]
+ Integrate1\output
+ motor_joint2\SignalLimiter2\output
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
+ motor_joint1\p2.e
+ motor_joint2\p2.e
@@ -3877,36 +3740,36 @@ implementation_end;
Camera Looking at Origin
- 3.61094505336573
+ 3.04292494817788
- 0.23725177442673
+ -0.198158420944799
- 0.233324732826921
+ 2.86669745787175
- -0.995780766056347
+ -0.727053165168863
- -0.065426294279019
+ 0.0473464543511176
- -0.0643433444044585
+ -0.684946719298893
- -0.0642049089065914
+ -0.6834989786941
- -0.00421848805226125
+ 0.0445101606651552
- 0.997927820050553
+ 0.728593159260835
Direct3D
@@ -4177,249 +4040,6 @@ implementation_end;
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
@@ -4476,11 +4096,11 @@ implementation_end;
new_joint1\position[2]
- 0.02758023578749541
+ 0.020422348089028722
new_joint1\position[3]
- -0.017200292914302247
+ 0.025208246380074812
@@ -4491,11 +4111,11 @@ implementation_end;
new_joint1\R[2,3]
- 0.5289972876475422
+ -0.7784945324835847
new_joint1\R[3,3]
- 0.8486235146762925
+ 0.6276513864345118
@@ -4505,11 +4125,11 @@ implementation_end;
new_joint1\R[2,2]
- 0.8486235146762925
+ 0.6276513864345118
new_joint1\R[3,2]
- -0.5289972876475422
+ 0.7784945324835847
Matrix
@@ -4595,11 +4215,11 @@ implementation_end;
new_joint2\position[2]
- 0.07637589665407551
+ 0.06513631397406291
new_joint2\position[3]
- -0.04762103432818519
+ 0.05636470076043039
@@ -4610,11 +4230,11 @@ implementation_end;
new_joint2\R[2,3]
- 0.5289973509267839
+ -0.23653295603011926
new_joint2\R[3,3]
- 0.8486234752305908
+ 0.971623466529938
@@ -4624,11 +4244,11 @@ implementation_end;
new_joint2\R[2,2]
- 0.8486234752305908
+ 0.971623466529938
new_joint2\R[3,2]
- -0.5289973509267839
+ 0.23653295603011926
Matrix
@@ -4729,8 +4349,8 @@ implementation_end;
- -0.1
- 0.1
+ -0.06
+ 0.14
true
1
@@ -4862,7 +4482,7 @@ implementation_end;
0.0
- 1.3
+ 3.0
true
3
@@ -4870,18 +4490,18 @@ implementation_end;
- -2.8
- -0.8000000000000003
+ -2.5
+ 2.5
true
- 2
-
+ 1
+
- -1885.20460252739
- 0.0
+ 0.0
+ 5.71937518881079
true
- 2
-
+ 1
+
@@ -4903,7 +4523,7 @@ implementation_end;
true
- PID2\error
+ motor_joint1\p2.e
@@ -4922,7 +4542,7 @@ implementation_end;
true
- PID3\error
+ motor_joint2\p2.e
@@ -5119,7 +4739,7 @@ implementation_end;
2
- false
+ true
Window 2
0
@@ -5129,7 +4749,7 @@ implementation_end;
3
- false
+ true
Window 3
0
@@ -5140,20 +4760,20 @@ implementation_end;
-0.0557292 0.189815 0.747917 0.775
+0.0651042 0.0583333 0.876042 0.85
-0.0807292 0.133333 0.720313 0.826852
+0.130729 0.0842593 0.846354 0.864815
-
-0.286979 0.255556 0.683333 0.693519
+
+0.109896 0.175926 0.50625 0.613889
0.0
- 0.1
+ 3.0
false
false
false