diff --git a/implementation/SCARA/05_stepper/stepper.emx b/implementation/SCARA/05_stepper/stepper.emx
index 8f3076b..04c8651 100644
--- a/implementation/SCARA/05_stepper/stepper.emx
+++ b/implementation/SCARA/05_stepper/stepper.emx
@@ -8,7 +8,7 @@
 <IsMainModel>0</IsMainModel>
   <KeepParameterValues>False</KeepParameterValues>
     <LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\05_stepper\stepper.emx</LibraryPath>
-  <TimeStamp>2020-7-22 16:26:16</TimeStamp>
+  <TimeStamp>2020-7-23 10:09:09</TimeStamp>
 <AllowLibraryUpdate>True</AllowLibraryUpdate>
  </Description>
  <Attributes>
@@ -31,7 +31,7 @@ initialequations
  end;
  implementation bg
   submodels
-   C 456 232
+   C 528 216
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -50,7 +50,7 @@ initialequations
     end;
     icon bg bottom
      figures
-      text 'C' 456 232 color 0 18 bold;
+      text 'C' 528 216 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -59,7 +59,7 @@ equations
 	state = int(p.f);
 	p.e = state / c;
 implementation_end;
-   C1 640 264
+   C1 712 232
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\3D\C-3.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:12:8</TimeStamp>
@@ -73,7 +73,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'C' 640 264 color 0 18 bold;
+      text 'C' 712 232 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -82,7 +82,7 @@ equations
     state = int(p.f);
     p.e = inverse(c) *state;
 implementation_end;
-   C2 304 272
+   C2 376 256
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\3D\C-3.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:12:8</TimeStamp>
@@ -96,7 +96,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'C' 304 272 color 0 18 bold;
+      text 'C' 376 256 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -105,7 +105,7 @@ equations
     state = int(p.f);
     p.e = inverse(c) *state;
 implementation_end;
-   C3 304 224
+   C3 376 208
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\2D\C-2.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:7:27</TimeStamp>
@@ -119,7 +119,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'C' 304 224 color 0 18 bold;
+      text 'C' 376 208 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -128,7 +128,7 @@ equations
     state = int(p.f);
     p.e = inverse(c) *state;
 implementation_end;
-   C4 640 216
+   C4 712 216
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\2D\C-2.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:7:27</TimeStamp>
@@ -140,9 +140,9 @@ implementation_end;
      restrictions
       causality preferred out p;
     end;
-    icon bg bottom
+    icon bg top
      figures
-      text 'C' 640 216 color 0 18 bold;
+      text 'C' 712 216 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -151,7 +151,7 @@ equations
     state = int(p.f);
     p.e = inverse(c) *state;
 implementation_end;
-   C5 32 232
+   C5 104 216
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -170,7 +170,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'C' 32 232 color 0 18 bold;
+      text 'C' 104 216 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -179,7 +179,7 @@ equations
 	state = int(p.f);
 	p.e = state / c;
 implementation_end;
-   COM 376 208
+   COM 448 192
     description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-21 12:24:38</TimeStamp></Description><Attributes><GlobalRelations>parameters
 	real I [3,1] = [1.6399999999999998e-6; 4.7e-8; 1.61e-6] {N.m.s};
 	real m = 0.00455 {kg};</GlobalRelations></Attributes></Information>';
@@ -191,11 +191,11 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      rectangle 344 192 408 224 color 0 fill 139;
-      text 'COM' 376 208 color 0 'Clear Sans' 16;
+      rectangle 416 176 480 208 color 0 fill 139;
+      text 'COM' 448 192 color 0 'Clear Sans' 16;
      terminals
-      Hin 392 192 fixed;
-      p 360 192 fixed;
+      Hin 464 176 fixed;
+      p 432 176 fixed;
     end;
     implementation bg
      submodels
@@ -399,7 +399,7 @@ implementation_end;
       ZeroJunction1\p => Ta0j\p;
      end;
     implementation_end;
-   COM1 720 200
+   COM1 792 184
     description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-21 12:24:38</TimeStamp></Description><Attributes><GlobalRelations>parameters
 	real I [3,1] = [7.583333333333335e-7; 3.645833333333334e-8; 7.364583333333335e-7] {N.m.s};
 	real m = 0.0035 {kg};</GlobalRelations></Attributes></Information>';
@@ -411,11 +411,11 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      rectangle 688 184 752 216 color 0 fill 139;
-      text 'COM' 720 200 color 0 'Clear Sans' 16;
+      rectangle 760 168 824 200 color 0 fill 139;
+      text 'COM' 792 184 color 0 'Clear Sans' 16;
      terminals
-      Hin 736 184 fixed;
-      p 704 184 fixed;
+      Hin 808 168 fixed;
+      p 776 168 fixed;
     end;
     implementation bg
      submodels
@@ -619,7 +619,7 @@ implementation_end;
       ZeroJunction1\p => Ta0j\p;
      end;
     implementation_end;
-   COM2 904 184
+   COM2 976 168
     description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-21 12:24:38</TimeStamp></Description><Attributes><GlobalRelations>parameters
 	real I [3,1] = [0.018;0.159;0.159] {mN.m.s};
 	real m = 0.015 {kg};</GlobalRelations></Attributes></Information>';
@@ -631,11 +631,11 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      rectangle 872 168 936 200 color 0 fill 139;
-      text 'COM' 904 184 color 0 'Clear Sans' 16;
+      rectangle 944 152 1008 184 color 0 fill 139;
+      text 'COM' 976 168 color 0 'Clear Sans' 16;
      terminals
-      Hin 920 168 fixed;
-      p 888 168 fixed;
+      Hin 992 152 fixed;
+      p 960 152 fixed;
     end;
     implementation bg
      submodels
@@ -839,7 +839,120 @@ implementation_end;
       ZeroJunction1\p => Ta0j\p;
      end;
     implementation_end;
-   inverse_kinematics1 272 552
+   Ground 144 104
+    description '<Information>
+ <Description>
+    <Version>4.8</Version>
+<IsMainModel>1</IsMainModel>
+    <LibraryPath>Bond Graph\MR\Ground.emx</LibraryPath>
+  <TimeStamp>2020-7-23 09:30:51</TimeStamp>
+</Description>
+</Information>';
+    type Submodel
+     ports
+      power in p [6,1];
+      signal out output [4,4];
+    end;
+    icon bg bottom
+     figures
+      line 108 96.5 124 112.5 color 0 fill 15132390;
+      line 116 96.5 132 112.5 color 0 fill 15132390;
+      line 100 96.5 116 112.5 color 0 fill 15132390;
+      line 140 112.5 124 96.5 color 0 fill 15132390;
+      line 148 112.5 132 96.5 color 0 fill 15132390;
+      line 156 112.5 140 96.5 color 0 fill 15132390;
+      line 172 112.5 156 96.5 color 0 fill 15132390;
+      line 164 112.5 148 96.5 color 0 fill 15132390;
+      line 180 112.5 164 96.5 color 0 fill 15132390;
+      line 188 112.5 172 96.5 color 0 fill 15132390;
+      line 100 96.5 188 96.5 color 0 fill 15132390 width 2;
+      line 180 96.5 188 104.5 color 0 fill 15132390;
+      line 100 104.5 108 112.5 color 0 fill 15132390;
+     terminals
+      p 124 96.5 fixed;
+      output 156 96.5 fixed;
+    end;
+    implementation bg
+     submodels
+      plug p 240 88;
+      plug output 240 120;
+      OneJunction2 192 88
+       description '<Information>
+ <Description>
+    <Version>4.2</Version>
+<IsMainModel>1</IsMainModel>
+  <KeepParameterValues>False</KeepParameterValues>
+    <LibraryPath>Bond Graph\OneJunction.emx</LibraryPath>
+  <TimeStamp>2011-11-29 16:17:51</TimeStamp>
+</Description>
+</Information>';
+       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' 192 88 color 0 18 bold;
+       end;
+       implementation eq
+equations
+	sum (direct (p.e)) = 0;
+	equal (collect (p.f));
+	flow = first (p.f);
+implementation_end;
+      Sf2 136 88
+       description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
+   <LibraryPath>Bond Graph\2D\Sf-2.emx</LibraryPath>
+  <TimeStamp>2007-9-25 12:7:5</TimeStamp>
+</Description>';
+       type 'Sf-2'
+        ports
+         power out p [6,1];
+        restrictions
+         causality fixed in p;
+       end;
+       icon bg bottom
+        figures
+         text 'Sf' 136 88 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;
+      Zero 144 120
+       description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
+  <LibraryPath>Signal\Sources\Zero.emx</LibraryPath>
+  <TimeStamp>2007-9-27 15:54:36</TimeStamp>
+</Description>
+';
+       type Zero
+        ports
+         signal out output [4,4];
+       end;
+       icon bg bottom
+        figures
+         rectangle 128.1 104 159.9 136 color 0 fill 15132390;
+         text '0' 144 120 color 16711680 18 bold;
+       end;
+       implementation eq
+equations
+	output = eye(4);implementation_end;
+     end;
+     connections
+      p => OneJunction2\p;
+      Sf2\p => OneJunction2\p;
+      Zero\output -> output;
+     end;
+    implementation_end;
+   inverse_kinematics1 344 536
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -884,7 +997,7 @@ equations
 	
 
  implementation_end;
-   Joint 192 120
+   Joint 264 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -904,13 +1017,13 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 168 88 216 152 color 0 fill 14745599;
-      text 'Joint' 192 120 color 0 'Clear Sans' 16;
+      rectangle 240 72 288 136 color 0 fill 14745599;
+      text 'Joint' 264 104 color 0 'Clear Sans' 16;
      terminals
-      Pin 216 104 fixed;
-      Hin 168 136 fixed;
-      Hout 216 136 fixed;
-      Pout 168 104 fixed;
+      Pin 288 88 fixed;
+      Hin 240 120 fixed;
+      Hout 288 120 fixed;
+      Pout 240 88 fixed;
     end;
     implementation bg
      submodels
@@ -1099,7 +1212,7 @@ implementation_end;
       Wbai\p => Pout;
      end;
     implementation_end;
-   Joint1 536 120
+   Joint1 608 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1119,13 +1232,13 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      rectangle 512 88 560 152 color 0 fill 14745599;
-      text 'Joint' 536 120 color 0 'Clear Sans' 16;
+      rectangle 584 72 632 136 color 0 fill 14745599;
+      text 'Joint' 608 104 color 0 'Clear Sans' 16;
      terminals
-      Pin 560 104 fixed;
-      Hin 512 136 fixed;
-      Hout 560 136 fixed;
-      Pout 512 104 fixed;
+      Pin 632 88 fixed;
+      Hin 584 120 fixed;
+      Hout 632 120 fixed;
+      Pout 584 88 fixed;
     end;
     implementation bg
      submodels
@@ -1314,7 +1427,7 @@ implementation_end;
       Wbai\p => Pout;
      end;
     implementation_end;
-   Link 288 120
+   Link 360 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1336,13 +1449,13 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      rectangle 256 96 320 144 color 0 fill 8454041;
-      text 'Link' 288 120 color 0 'Clear Sans' 16;
+      rectangle 328 80 392 128 color 0 fill 8454041;
+      text 'Link' 360 104 color 0 'Clear Sans' 16;
      terminals
-      Hin 256 136 fixed;
-      Hout 320 136 fixed;
-      Pin 320 104 fixed;
-      Pout 256 104 fixed;
+      Hin 328 120 fixed;
+      Hout 392 120 fixed;
+      Pin 392 88 fixed;
+      Pout 328 88 fixed;
     end;
     implementation eq
 parameters
@@ -1363,7 +1476,7 @@ equations
 	Pin.f = AdHab * Pout.f;
 
     implementation_end;
-   Link1 456 120
+   Link1 528 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1385,13 +1498,13 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 424 96 488 144 color 0 fill 8454041;
-      text 'Link' 456 120 color 0 'Clear Sans' 16;
+      rectangle 496 80 560 128 color 0 fill 8454041;
+      text 'Link' 528 104 color 0 'Clear Sans' 16;
      terminals
-      Hin 424 136 fixed;
-      Hout 488 136 fixed;
-      Pin 488 104 fixed;
-      Pout 424 104 fixed;
+      Hin 496 120 fixed;
+      Hout 560 120 fixed;
+      Pin 560 88 fixed;
+      Pout 496 88 fixed;
     end;
     implementation eq
 parameters
@@ -1412,7 +1525,7 @@ equations
 	Pin.f = AdHab * Pout.f;
 
     implementation_end;
-   Link2 624 120
+   Link2 696 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1434,13 +1547,13 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 592 96 656 144 color 0 fill 8454041;
-      text 'Link' 624 120 color 0 'Clear Sans' 16;
+      rectangle 664 80 728 128 color 0 fill 8454041;
+      text 'Link' 696 104 color 0 'Clear Sans' 16;
      terminals
-      Hin 592 136 fixed;
-      Hout 656 136 fixed;
-      Pin 656 104 fixed;
-      Pout 592 104 fixed;
+      Hin 664 120 fixed;
+      Hout 728 120 fixed;
+      Pin 728 88 fixed;
+      Pout 664 88 fixed;
     end;
     implementation eq
 parameters
@@ -1456,7 +1569,7 @@ equations
 	Pin.f = AdHab * Pout.f;
 
     implementation_end;
-   Link3 800 120
+   Link3 872 104
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1478,13 +1591,13 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 768 96 832 144 color 0 fill 8454041;
-      text 'Link' 800 120 color 0 'Clear Sans' 16;
+      rectangle 840 80 904 128 color 0 fill 8454041;
+      text 'Link' 872 104 color 0 'Clear Sans' 16;
      terminals
-      Hin 768 136 fixed;
-      Hout 832 136 fixed;
-      Pin 832 104 fixed;
-      Pout 768 104 fixed;
+      Hin 840 120 fixed;
+      Hout 904 120 fixed;
+      Pin 904 88 fixed;
+      Pout 840 88 fixed;
     end;
     implementation eq
 parameters
@@ -1500,7 +1613,7 @@ equations
 	Pin.f = AdHab * Pout.f;
 
     implementation_end;
-   Negate1 272 472
+   Negate1 344 456
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Signal\Block Diagram\Negate.emx</LibraryPath>
   <TimeStamp>2007-9-26 12:14:11</TimeStamp>
@@ -1512,14 +1625,14 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 256 456 288 488 color 0 fill 15132390;
-      text '-1' 272 472 color 16711680 16 bold;
+      rectangle 328 440 360 472 color 0 fill 15132390;
+      text '-1' 344 456 color 16711680 16 bold;
     end;
     implementation eq
 equations
 	output = - input;
      implementation_end;
-   Negate2 520 552
+   Negate2 592 536
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Signal\Block Diagram\Negate.emx</LibraryPath>
   <TimeStamp>2007-9-26 12:14:11</TimeStamp>
@@ -1531,14 +1644,14 @@ equations
     end;
     icon bg bottom
      figures
-      rectangle 504 536 536 568 color 0 fill 15132390;
-      text '-1' 520 552 color 16711680 16 bold;
+      rectangle 576 520 608 552 color 0 fill 15132390;
+      text '-1' 592 536 color 16711680 16 bold;
     end;
     implementation eq
 equations
 	output = - input;
      implementation_end;
-   new_joint1 376 280
+   new_joint1 448 264
     description '<Description><Version>4.0</Version>
    <LibraryPath>Template\Submodel-Equation.emx</LibraryPath>
 <IsMainModel>1</IsMainModel>
@@ -1560,7 +1673,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 720 256
+   new_joint2 792 240
     description '<Description><Version>4.0</Version>
    <LibraryPath>Template\Submodel-Equation.emx</LibraryPath>
 <IsMainModel>1</IsMainModel>
@@ -1582,7 +1695,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 1016 184
+   new_joint3 1088 168
     description '<Description><Version>4.0</Version>
    <LibraryPath>Template\Submodel-Equation.emx</LibraryPath>
 <IsMainModel>1</IsMainModel>
@@ -1603,7 +1716,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;
-   OneJunction1 360 104
+   OneJunction1 432 88
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1622,7 +1735,7 @@ equations
     end;
     icon bg
      figures
-      text '1' 360 104 color 0 18 bold;
+      text '1' 432 88 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1630,7 +1743,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction11 520 352
+   OneJunction11 592 336
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1649,7 +1762,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 520 352 color 0 18 bold;
+      text '1' 592 336 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1657,7 +1770,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction12 888 104
+   OneJunction12 960 88
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1676,7 +1789,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 888 104 color 0 18 bold;
+      text '1' 960 88 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1684,7 +1797,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction14 112 352
+   OneJunction14 184 328
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1703,7 +1816,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 112 352 color 0 18 bold;
+      text '1' 184 328 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1711,7 +1824,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction17 456 280
+   OneJunction17 528 264
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1730,34 +1843,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 456 280 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 '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\OneJunction.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:17:51</TimeStamp>
-</Description>
-</Information>';
-    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;
+      text '1' 528 264 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1765,7 +1851,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction3 704 104
+   OneJunction3 776 88
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1784,7 +1870,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 704 104 color 0 18 bold;
+      text '1' 776 88 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1792,7 +1878,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction4 593 232
+   OneJunction4 664 216
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1811,7 +1897,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 593 232 color 0 18 bold;
+      text '1' 664 216 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1819,7 +1905,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction5 257 240
+   OneJunction5 329 224
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1838,7 +1924,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 257 240 color 0 18 bold;
+      text '1' 329 224 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1846,7 +1932,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction6 257 256
+   OneJunction6 329 240
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1865,7 +1951,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 257 256 color 0 18 bold;
+      text '1' 329 240 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1873,7 +1959,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction7 593 248
+   OneJunction7 664 232
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1892,7 +1978,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 593 248 color 0 18 bold;
+      text '1' 664 232 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1900,7 +1986,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction8 32 280
+   OneJunction8 104 264
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1919,7 +2005,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 32 280 color 0 18 bold;
+      text '1' 104 264 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1927,7 +2013,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   PowerMux 192 214
+   PowerMux 264 198
 			specifications active 'rot_x'
 				specification 'rot_x'
  description '<Information>
@@ -1953,14 +2039,14 @@ implementation_end;
  end;
  icon bg bottom
   figures
-   line 168 216 216 216 color 0 width 2;
-   rectangle 168 208 216 220 color -1;
-   text '1' 186 211 color 8421504 8;
+   line 240 200 288 200 color 0 width 2;
+   rectangle 240 192 288 204 color -1;
+   text '1' 258 195 color 8421504 8;
   terminals
-   input 176 216 fixed;
-   output 192 216 fixed;
-   input_rot2 208 216 fixed;
-   input_pos3 192 216 fixed;
+   input 248 200 fixed;
+   output 264 200 fixed;
+   input_rot2 280 200 fixed;
+   input_pos3 264 200 fixed;
  end;
  implementation eq
 equations
@@ -2050,7 +2136,7 @@ equations
 	output.f[4:6] = input_pos3.f;implementation_end;
 specification_end;
 			end;
-   PowerMux1 536 206
+   PowerMux1 608 190
 			specifications active 'rot_x'
 				specification 'rot_x'
  description '<Information>
@@ -2076,14 +2162,14 @@ specification_end;
  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;
+   line 584 192 632 192 color 0 width 2;
+   rectangle 584 184 632 196 color -1;
+   text '1' 602 187 color 8421504 8;
   terminals
-   input 520 208 fixed;
-   output 536 208 fixed;
-   input_rot2 552 208 fixed;
-   input_pos3 536 208 fixed;
+   input 592 192 fixed;
+   output 608 192 fixed;
+   input_rot2 624 192 fixed;
+   input_pos3 608 192 fixed;
  end;
  implementation eq
 equations
@@ -2173,7 +2259,7 @@ equations
 	output.f[4:6] = input_pos3.f;implementation_end;
 specification_end;
 			end;
-   R1 456 328
+   R1 528 312
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -2189,7 +2275,7 @@ specification_end;
     end;
     icon bg bottom
      figures
-      text 'R' 456 328 color 0 18 bold;
+      text 'R' 528 312 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2197,7 +2283,7 @@ parameters
 equations
 	p.e = r * p.f;
 implementation_end;
-   R2 257 184
+   R2 329 168
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\2D\R-2.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:6:54</TimeStamp>
@@ -2208,7 +2294,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'R' 257 184 color 0 18 bold;
+      text 'R' 329 168 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2216,7 +2302,7 @@ parameters
 equations					
 	p.e = r * p.f;
 implementation_end;
-   R3 257 312
+   R3 329 296
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\3D\R-3.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:11:54</TimeStamp>
@@ -2227,7 +2313,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'R' 257 312 color 0 18 bold;
+      text 'R' 329 296 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2235,7 +2321,7 @@ parameters
 equations					
 	p.e = r * p.f;
 implementation_end;
-   R4 593 176
+   R4 664 160
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\2D\R-2.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:6:54</TimeStamp>
@@ -2244,9 +2330,9 @@ implementation_end;
      ports
       power in p [2,1];
     end;
-    icon bg bottom
+    icon bg top
      figures
-      text 'R' 593 176 color 0 18 bold;
+      text 'R' 664 160 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2254,7 +2340,7 @@ parameters
 equations					
 	p.e = r * p.f;
 implementation_end;
-   R5 593 304
+   R5 664 280
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\3D\R-3.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:11:54</TimeStamp>
@@ -2265,7 +2351,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'R' 593 304 color 0 18 bold;
+      text 'R' 664 280 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2273,7 +2359,7 @@ parameters
 equations					
 	p.e = r * p.f;
 implementation_end;
-   R6 32 328
+   R6 104 312
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -2289,7 +2375,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'R' 32 328 color 0 18 bold;
+      text 'R' 104 312 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -2297,7 +2383,7 @@ parameters
 equations
 	p.e = r * p.f;
 implementation_end;
-   rectanglepath 104 552
+   rectanglepath 176 536
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -2335,7 +2421,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 656
+   rectanglepath1 344 640
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -2352,8 +2438,8 @@ implementation_end;
     end;
     icon bg
      figures
-      rectangle 216 640 328 672 color 0 fill 15132390;
-      text 'name' 272 656 color 0 'Clear Sans' 16;
+      rectangle 288 624 400 656 color 0 fill 15132390;
+      text 'name' 344 640 color 0 'Clear Sans' 16;
     end;
     implementation eq
 /* 
@@ -2397,31 +2483,7 @@ 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 '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
-   <LibraryPath>Bond Graph\2D\Sf-2.emx</LibraryPath>
-  <TimeStamp>2007-9-25 12:7:5</TimeStamp>
-</Description>';
-    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
+   Splitter1 464 120
     description '<Description><Version>4.0</Version>
    <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
   <TimeStamp>2008-01-17 11:28:29</TimeStamp>
@@ -2435,16 +2497,16 @@ implementation_end;
     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;
+      ellipse 460.8 116.8 467.2 123.2 color -1 fill 0;
+      ellipse 459.7 115.7 468.3 124.3 color -1;
      terminals
-      input 392 136 fixed;
+      input 464 120 fixed;
     end;
     implementation eq
 equations
     collect (output) = input;
 implementation_end;
-   Splitter2 736 136
+   Splitter2 808 120
     description '<Description><Version>4.0</Version>
    <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
   <TimeStamp>2008-01-17 11:28:29</TimeStamp>
@@ -2458,16 +2520,16 @@ implementation_end;
     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;
+      ellipse 804.8 116.8 811.2 123.2 color -1 fill 0;
+      ellipse 803.7 115.7 812.3 124.3 color -1;
      terminals
-      input 736 136 fixed;
+      input 808 120 fixed;
     end;
     implementation eq
 equations
     collect (output) = input;
 implementation_end;
-   stepper_control 112 472
+   stepper_control 184 456
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -2756,7 +2818,7 @@ equations
       Velocity_limit\output -> PlusMinus2\plus;
      end;
     implementation_end;
-   stepper_control1 520 472
+   stepper_control1 592 456
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -3045,7 +3107,7 @@ equations
       Velocity_limit\output -> PlusMinus2\plus;
      end;
     implementation_end;
-   stepper_model 112 392
+   stepper_model 184 376
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -3583,7 +3645,7 @@ implementation_end;
       Se\p => OneJunction4\p;
      end;
     implementation_end;
-   stepper_model1 520 392
+   stepper_model1 592 376
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -4121,25 +4183,7 @@ implementation_end;
       Se\p => OneJunction4\p;
      end;
     implementation_end;
-   Zero 112 136
-    description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
-  <LibraryPath>Signal\Sources\Zero.emx</LibraryPath>
-  <TimeStamp>2007-9-27 15:54:36</TimeStamp>
-</Description>
-';
-    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;
-   ZeroJunction2 520 280
+   ZeroJunction2 592 264
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -4158,7 +4202,7 @@ equations
     end;
     icon bg
      figures
-      text '0' 520 280 color 0 18 bold;
+      text '0' 592 264 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -4166,7 +4210,7 @@ equations
 	equal (collect (p.e));
 	effort = first (p.e);
 implementation_end;
-   ZeroJunction3 112 288
+   ZeroJunction3 184 264
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -4185,7 +4229,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '0' 112 288 color 0 18 bold;
+      text '0' 184 264 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -4199,15 +4243,16 @@ implementation_end;
    C2\p <= OneJunction6\p;
    COM\Hout -> new_joint1\input;
    COM1\Hout -> new_joint2\input;
-   COM2\Hin <- Link3\Hout 920 136;
+   COM2\Hin <- Link3\Hout 992 120;
    COM2\Hout -> new_joint3\input;
    COM2\p => OneJunction12\p;
+   Ground\output -> Joint\Hin;
    inverse_kinematics1\angle1 -> Negate1\input;
    inverse_kinematics1\angle2 -> Negate2\input;
    Joint\Hout -> Link\Hin;
    Joint\Pdiff <= PowerMux\output;
    Joint\Pin <= Link\Pout;
-   Joint\Pout => OneJunction2\p;
+   Joint\Pout => Ground\p;
    Link\Pin <= OneJunction1\p;
    Link1\Hout -> Joint1\Hin;
    Link1\Pin <= Joint1\Pout;
@@ -4240,7 +4285,6 @@ implementation_end;
    R1\p <= OneJunction17\p;
    R6\p <= OneJunction8\p;
    rectanglepath1\output -> inverse_kinematics1\input;
-   Sf2\p => OneJunction2\p;
    Splitter1\input <- Link\Hout;
    Splitter1\output -> COM\Hin;
    Splitter1\output -> Link1\Hin;
@@ -4250,7 +4294,6 @@ implementation_end;
    stepper_control1\output -> stepper_model1\angle;
    stepper_model\p => OneJunction14\p;
    stepper_model1\p => OneJunction11\p;
-   Zero\output -> Joint\Hin;
    ZeroJunction2\p => OneJunction17\p;
    ZeroJunction2\p => PowerMux1\input;
    ZeroJunction3\p => OneJunction8\p;
@@ -4364,6 +4407,10 @@ implementation_end;
      <Name>stepper_model1\I1\state_initial</Name>
      <Value>0</Value>
     </Variable>
+    <Variable>
+     <Name>stepper_model1\I2\state_initial</Name>
+     <Value>0</Value>
+    </Variable>
     <Variable>
      <Name>stepper_model1\I\state_initial</Name>
      <Value>0</Value>
@@ -4377,6 +4424,10 @@ implementation_end;
      <Name>stepper_model\I1\state_initial</Name>
      <Value>0</Value>
     </Variable>
+    <Variable>
+     <Name>stepper_model\I2\state_initial</Name>
+     <Value>0</Value>
+    </Variable>
     <Variable>
      <Name>stepper_model\I\state_initial</Name>
      <Value>0</Value>
@@ -4392,14 +4443,6 @@ implementation_end;
      <Columns>1</Columns>
      <Value>0 0 0 0 0 0</Value>
     </Variable>
-    <Variable>
-     <Name>stepper_model1\I2\p.e_initial</Name>
-     <Value>0</Value>
-    </Variable>
-    <Variable>
-     <Name>stepper_model\I2\p.e_initial</Name>
-     <Value>0</Value>
-    </Variable>
    </Initials>
   </Variables>
   <PlotSpecs>
@@ -4427,7 +4470,12 @@ implementation_end;
     <VarName>new_joint3\position[2]</VarName>
     <VarName>rectanglepath1\output[2]</VarName>
     <VarName>rectanglepath1\output[1]</VarName>
-    <VarName>Integrate2\output</VarName>
+    <VarName>stepper_control\Acceleration_int\output</VarName>
+    <VarName>stepper_control\Acceleration_int\input</VarName>
+    <VarName>stepper_control\Velocity_int\output</VarName>
+    <VarName>stepper_control1\Acceleration_int\output</VarName>
+    <VarName>stepper_control1\Acceleration_int\input</VarName>
+    <VarName>stepper_control1\Velocity_int\output</VarName>
     <VarName>Submodel3\dimension[1]</VarName>
     <VarName>Submodel3\dimension[2]</VarName>
     <VarName>Submodel3\dimension[3]</VarName>
@@ -4435,13 +4483,8 @@ implementation_end;
     <VarName>Submodel4\dimension[2]</VarName>
     <VarName>Submodel4\dimension[3]</VarName>
     <VarName>Step\output</VarName>
+    <VarName>Integrate2\output</VarName>
     <VarName>motor_joint2\SignalLimiter2\output</VarName>
-    <VarName>stepper_control\Acceleration_int\output</VarName>
-    <VarName>stepper_control\Acceleration_int\input</VarName>
-    <VarName>stepper_control\Velocity_int\output</VarName>
-    <VarName>stepper_control1\Acceleration_int\output</VarName>
-    <VarName>stepper_control1\Acceleration_int\input</VarName>
-    <VarName>stepper_control1\Velocity_int\output</VarName>
    </VarNames>
    <Plots>
     <Plot>
@@ -5410,11 +5453,11 @@ implementation_end;
          </X>
          <Y>
      <VariableName>new_joint1\position[2]</VariableName>
-     <Value>-0.01651527795907241</Value>
+     <Value>0.004779004084536204</Value>
          </Y>
          <Z>
      <VariableName>new_joint1\position[3]</VariableName>
-     <Value>0.027946459849279154</Value>
+     <Value>0.032131984037075616</Value>
          </Z>
         </Position>
         <Orientation>
@@ -5425,11 +5468,11 @@ implementation_end;
      </X>
      <Y>
       <VariableName>new_joint1\R[2,3]</VariableName>
-      <Value>-0.8611149858648751</Value>
+      <Value>-0.9891507613546217</Value>
      </Y>
      <Z>
       <VariableName>new_joint1\R[3,3]</VariableName>
-      <Value>-0.5084102488334947</Value>
+      <Value>0.14690395267511475</Value>
      </Z>
          </ZAxis>
          <YAxis>
@@ -5439,11 +5482,11 @@ implementation_end;
      </X>
      <Y>
       <VariableName>new_joint1\R[2,2]</VariableName>
-      <Value>-0.5084102488334947</Value>
+      <Value>0.14690395267511475</Value>
      </Y>
      <Z>
       <VariableName>new_joint1\R[3,2]</VariableName>
-      <Value>0.8611149858648751</Value>
+      <Value>0.9891507613546217</Value>
      </Z>
          </YAxis>
          <OrientationType>Matrix</OrientationType>
@@ -5529,11 +5572,11 @@ implementation_end;
          </X>
          <Y>
      <VariableName>new_joint2\position[2]</VariableName>
-     <Value>-0.008092142484759316</Value>
+     <Value>0.03341576943552989</Value>
          </Y>
          <Z>
      <VariableName>new_joint2\position[3]</VariableName>
-     <Value>0.05482151967891108</Value>
+     <Value>0.05682777438151479</Value>
          </Z>
         </Position>
         <Orientation>
@@ -5544,11 +5587,11 @@ implementation_end;
      </X>
      <Y>
       <VariableName>new_joint2\R[2,3]</VariableName>
-      <Value>0.04312796974618256</Value>
+      <Value>0.29812485436275077</Value>
      </Y>
      <Z>
       <VariableName>new_joint2\R[3,3]</VariableName>
-      <Value>0.9990695562500003</Value>
+      <Value>0.9545268834407906</Value>
      </Z>
          </ZAxis>
          <YAxis>
@@ -5558,11 +5601,11 @@ implementation_end;
      </X>
      <Y>
       <VariableName>new_joint2\R[2,2]</VariableName>
-      <Value>0.9990695562500003</Value>
+      <Value>0.9545268834407906</Value>
      </Y>
      <Z>
       <VariableName>new_joint2\R[3,2]</VariableName>
-      <Value>-0.043127969746182555</Value>
+      <Value>-0.29812485436275077</Value>
      </Z>
          </YAxis>
          <OrientationType>Matrix</OrientationType>
@@ -5663,8 +5706,8 @@ implementation_end;
      </XAxes>
      <YAxes>
       <Axis>
-       <Minimum>-0.06</Minimum>
-       <Maximum>0.14</Maximum>
+       <Minimum>-0.020000000000000004</Minimum>
+       <Maximum>0.07999999999999999</Maximum>
        <Linear>true</Linear>
        <Scaling>1</Scaling>
        <Label>Simulated path</Label>
@@ -5968,7 +6011,7 @@ implementation_end;
      <XAxes>
       <Axis>
        <Minimum>0.0</Minimum>
-       <Maximum>6.088762158038446</Maximum>
+       <Maximum>0.13434062268478064</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>time</Label>
@@ -6178,7 +6221,7 @@ implementation_end;
    </PlotPanels>
    <WindowRectangles virtualDesktopWidth="5760" virtualDesktopHeight="1080">
      <WindowRectangle monitor="2" left="1920" top="0" right="3840" bottom="1080" plotpanelid="1">
-0.0703125 0.0435185 0.872917 0.814815
+0.0744792 0.00462963 0.86875 0.755556
      </WindowRectangle>
      <WindowRectangle monitor="1" left="-1920" top="0" right="0" bottom="1080" plotpanelid="2">
 0.130729 0.0842593 0.846354 0.864815