Update motor Physics model

implementation/SCARA/03_motor/motor_physics2.emx

@@ -8,7 +8,7 @@
 <IsMainModel>0</IsMainModel>
   <KeepParameterValues>False</KeepParameterValues>
     <LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\03_motor\motor_physics2.emx</LibraryPath>
-  <TimeStamp>2020-7-20 15:06:45</TimeStamp>
+  <TimeStamp>2020-7-20 15:58:21</TimeStamp>
 <AllowLibraryUpdate>True</AllowLibraryUpdate>
  </Description>
  <Attributes>
@@ -32,9 +32,9 @@ initialequations
  implementation bg
   submodels
    Base1 720 184
-    description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><AllowLibraryUpdate>True</AllowLibraryUpdate><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-20 14:44:13</TimeStamp><Description>Base</Description></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>';
+    description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><AllowLibraryUpdate>True</AllowLibraryUpdate><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-20 15:56:10</TimeStamp><Description>Base</Description></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>';
     type Submodel
      ports
       power out p [6,1];
@@ -54,20 +54,12 @@ initialequations
     implementation bg
      submodels
       AdHi0 544 400
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\MTF.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:15:31</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:3:3</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type MTF
         ports
-         power in p1;
-         power out p2;
-         signal in r;
+         power in p1 [6,1];
+         power out p2 [6,1];
+         signal in H [4,4];
         restrictions
          causality constraint not_equal p1 p2;
        end;
@@ -76,50 +68,51 @@ initialequations
          text 'MTF' 544 400 color 0 18 bold;
        end;
        implementation eq
-equations
-	p1.e = r * p2.e;
-	p2.f = r * p1.f;
+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 '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\MGY.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:03:53</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-10-31 11:43:6</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type MGY
         ports
-         power in p1;
-         power out p2;
-         signal in r;
-        restrictions
-         causality constraint equal p1 p2;
+         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
-	p1.e = r * p2.f;
-	p2.e = r * p1.f;
-implementation_end;
+	Q = transpose(adjoint(p1.f));
+	QI = Q*II;
+	p1.e = QI*p1.f;implementation_end;
       Gravity 616 400
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\Se.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:36:49</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:3:26</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type Se
         ports
-         power out p;
+         power out p [6,1];
         restrictions
          causality fixed out p;
        end;
@@ -129,27 +122,19 @@ implementation_end;
        end;
        implementation eq
 parameters
-	real effort = 1;
+	real global m;
 variables
-	real flow;
+	real effort[6];
 equations
+	effort = [0;0;0;0;0;-g_n*m];
 	p.e = effort;
-	flow = p.f;
 implementation_end;
       InertialTensor 544 288
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\I.emx</LibraryPath>
-  <TimeStamp>2011-11-29 15:55:55</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:2:12</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type I
         ports
-         power in p;
-         signal out state;
+         power in p [6,1];
+         signal out state [6,1];
         restrictions
          causality preferred in p;
        end;
@@ -159,10 +144,18 @@ implementation_end;
        end;
        implementation eq
 parameters
-	real i = 1;
+	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);
-    p.f = state / i;
+    state = int(p.e); //state = generalized momentum
+    p.f = inverse(II)*state;
 implementation_end;
       plug p 472 336;
       plug Hin 472 496;
@@ -191,19 +184,19 @@ equations
     collect (output) = input;
 implementation_end;
       Ta0j 544 336
-       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>';
+       description '<Description>
+ <Version>4.0</Version>
+ <IsMainModel>1</IsMainModel>
+ <KeepParameterValues>False</KeepParameterValues>
+ <LibraryPath>Bond Graph\OneJunction.emx</LibraryPath>
+ <TimeStamp>2007-9-27 9:51:18</TimeStamp>
+ <AllowLibraryUpdate>True</AllowLibraryUpdate>
+   <Description></Description>
+</Description>';
        knot OneJunction
         ports
-         power knot duplicatable none p [1];
-         signal knot out flow [1];
+         power knot duplicatable none p [6,1];
+         signal knot out flow [6,1];
         restrictions
          causality constraint one_out p;
        end;
@@ -224,6 +217,7 @@ implementation_end;
       Hin -> Splitter1\input;
       InertialTensor\p <= Ta0j\p;
       p <= Ta0j\p;
+      Splitter1\output -> AdHi0\H;
       Splitter1\output -> Hout;
       Ta0j\p => EJS\p1;
      end;
@@ -246,9 +240,9 @@ k = Body fixed frame, principal inertial frame
  ' 288 280 color 0;
     implementation_end;
    Base2 912 120
-    description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><AllowLibraryUpdate>True</AllowLibraryUpdate><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-20 14:44:13</TimeStamp><Description>Base</Description></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>';
+    description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><AllowLibraryUpdate>True</AllowLibraryUpdate><LibraryPath>Bond Graph\MR\center_of_mass_v2.emx</LibraryPath><TimeStamp>2020-7-20 15:56:10</TimeStamp><Description>Base</Description></Description><Attributes><GlobalRelations>parameters
+	real I [3,1] = [0;0;0] {N.m.s};
+	real m = 0.015 {kg};</GlobalRelations></Attributes></Information>';
     type Submodel
      ports
       power out p [6,1];
@@ -258,30 +252,22 @@ k = Body fixed frame, principal inertial frame
     icon bg ellipse bottom
      figures
       ellipse 888 96 936 144 color 0 fill 16777215 width 2;
-      line 912 96 912 144 color 0 fill 16777215;
       line 888 120 936 120 color 0 fill 16777215;
+      line 912 96 912 144 color 0 fill 16777215;
      terminals
-      p 888 104 fixed;
-      Hin 888 136 fixed;
+      p 896 96 fixed;
+      Hin 928 96 fixed;
       Hout 936 152 fixed;
     end;
     implementation bg
      submodels
       AdHi0 544 400
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\MTF.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:15:31</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:3:3</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type MTF
         ports
-         power in p1;
-         power out p2;
-         signal in r;
+         power in p1 [6,1];
+         power out p2 [6,1];
+         signal in H [4,4];
         restrictions
          causality constraint not_equal p1 p2;
        end;
@@ -290,50 +276,51 @@ k = Body fixed frame, principal inertial frame
          text 'MTF' 544 400 color 0 18 bold;
        end;
        implementation eq
-equations
-	p1.e = r * p2.e;
-	p2.f = r * p1.f;
+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 '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\MGY.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:03:53</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-10-31 11:43:6</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type MGY
         ports
-         power in p1;
-         power out p2;
-         signal in r;
-        restrictions
-         causality constraint equal p1 p2;
+         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
-	p1.e = r * p2.f;
-	p2.e = r * p1.f;
-implementation_end;
+	Q = transpose(adjoint(p1.f));
+	QI = Q*II;
+	p1.e = QI*p1.f;implementation_end;
       Gravity 616 400
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\Se.emx</LibraryPath>
-  <TimeStamp>2011-11-29 16:36:49</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:3:26</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type Se
         ports
-         power out p;
+         power out p [6,1];
         restrictions
          causality fixed out p;
        end;
@@ -343,27 +330,19 @@ implementation_end;
        end;
        implementation eq
 parameters
-	real effort = 1;
+	real global m;
 variables
-	real flow;
+	real effort[6];
 equations
+	effort = [0;0;0;0;0;-g_n*m];
 	p.e = effort;
-	flow = p.f;
 implementation_end;
       InertialTensor 544 288
-       description '<Information>
- <Description>
-    <Version>4.2</Version>
-<IsMainModel>1</IsMainModel>
-  <KeepParameterValues>False</KeepParameterValues>
-    <LibraryPath>Bond Graph\I.emx</LibraryPath>
-  <TimeStamp>2011-11-29 15:55:55</TimeStamp>
-</Description>
-</Information>';
+       description '<Information><Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues><LibraryPath></LibraryPath><TimeStamp>2007-9-25 12:2:12</TimeStamp><AllowLibraryUpdate>True</AllowLibraryUpdate></Description></Information>';
        type I
         ports
-         power in p;
-         signal out state;
+         power in p [6,1];
+         signal out state [6,1];
         restrictions
          causality preferred in p;
        end;
@@ -373,10 +352,18 @@ implementation_end;
        end;
        implementation eq
 parameters
-	real i = 1;
+	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);
-    p.f = state / i;
+    state = int(p.e); //state = generalized momentum
+    p.f = inverse(II)*state;
 implementation_end;
       plug p 472 336;
       plug Hin 472 496;
@@ -405,19 +392,19 @@ equations
     collect (output) = input;
 implementation_end;
       Ta0j 544 336
-       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>';
+       description '<Description>
+ <Version>4.0</Version>
+ <IsMainModel>1</IsMainModel>
+ <KeepParameterValues>False</KeepParameterValues>
+ <LibraryPath>Bond Graph\OneJunction.emx</LibraryPath>
+ <TimeStamp>2007-9-27 9:51:18</TimeStamp>
+ <AllowLibraryUpdate>True</AllowLibraryUpdate>
+   <Description></Description>
+</Description>';
        knot OneJunction
         ports
-         power knot duplicatable none p [1];
-         signal knot out flow [1];
+         power knot duplicatable none p [6,1];
+         signal knot out flow [6,1];
         restrictions
          causality constraint one_out p;
        end;
@@ -438,6 +425,7 @@ implementation_end;
       Hin -> Splitter1\input;
       InertialTensor\p <= Ta0j\p;
       p <= Ta0j\p;
+      Splitter1\output -> AdHi0\H;
       Splitter1\output -> Hout;
       Ta0j\p => EJS\p1;
      end;
@@ -2980,24 +2968,6 @@ implementation_end;
   <PlotSpecs>
    <VarNames>
     <VarName>time</VarName>
-    <VarName>Submodel3\body_position[1]</VarName>
-    <VarName>Submodel3\body_position[2]</VarName>
-    <VarName>Submodel3\body_position[3]</VarName>
-    <VarName>Submodel3\body_angle[1]</VarName>
-    <VarName>Submodel3\body_angle[2]</VarName>
-    <VarName>Submodel3\body_angle[3]</VarName>
-    <VarName>Submodel3\dimension[1]</VarName>
-    <VarName>Submodel3\dimension[2]</VarName>
-    <VarName>Submodel3\dimension[3]</VarName>
-    <VarName>Submodel4\dimension[1]</VarName>
-    <VarName>Submodel4\dimension[2]</VarName>
-    <VarName>Submodel4\dimension[3]</VarName>
-    <VarName>Submodel4\body_angle[1]</VarName>
-    <VarName>Submodel4\body_angle[2]</VarName>
-    <VarName>Submodel4\body_angle[3]</VarName>
-    <VarName>Submodel4\body_position[1]</VarName>
-    <VarName>Submodel4\body_position[2]</VarName>
-    <VarName>Submodel4\body_position[3]</VarName>
     <VarName>new_joint1\position[1]</VarName>
     <VarName>new_joint1\position[2]</VarName>
     <VarName>new_joint1\position[3]</VarName>
@@ -3022,12 +2992,30 @@ implementation_end;
     <VarName>new_joint3\position[2]</VarName>
     <VarName>rectanglepath\output[2]</VarName>
     <VarName>rectanglepath\output[1]</VarName>
+    <VarName>Submodel3\body_position[1]</VarName>
+    <VarName>Submodel3\body_position[2]</VarName>
+    <VarName>Submodel3\body_position[3]</VarName>
+    <VarName>Submodel3\body_angle[1]</VarName>
+    <VarName>Submodel3\body_angle[2]</VarName>
+    <VarName>Submodel3\body_angle[3]</VarName>
+    <VarName>Submodel3\dimension[1]</VarName>
+    <VarName>Submodel3\dimension[2]</VarName>
+    <VarName>Submodel3\dimension[3]</VarName>
+    <VarName>Submodel4\body_position[1]</VarName>
+    <VarName>Submodel4\body_position[2]</VarName>
+    <VarName>Submodel4\body_position[3]</VarName>
+    <VarName>Submodel4\body_angle[1]</VarName>
+    <VarName>Submodel4\body_angle[2]</VarName>
+    <VarName>Submodel4\body_angle[3]</VarName>
+    <VarName>Submodel4\dimension[1]</VarName>
+    <VarName>Submodel4\dimension[2]</VarName>
+    <VarName>Submodel4\dimension[3]</VarName>
    </VarNames>
    <Plots>
     <Plot>
      <PlotType>D3DPlot</PlotType>
      <BasePlot>
-       <PlotId>3</PlotId>
+       <PlotId>1</PlotId>
        <UseWindowsBGColor>false</UseWindowsBGColor>
        <BGColor>16777215</BGColor>
        <PlotIsVisible>true</PlotIsVisible>
@@ -3646,7 +3634,7 @@ implementation_end;
       <Orientation>
        <ZAxis>
         <X>
-         <Value>-0.951853185182918</Value>
+         <Value>-0.951853185182919</Value>
         </X>
         <Y>
          <Value>-0.0364571532071126</Value>
@@ -4410,7 +4398,7 @@ implementation_end;
     <Plot>
      <PlotType>GraphPlot</PlotType>
      <BasePlot>
-       <PlotId>4</PlotId>
+       <PlotId>2</PlotId>
        <UseWindowsBGColor>false</UseWindowsBGColor>
        <BGColor>16777215</BGColor>
        <PlotIsVisible>true</PlotIsVisible>
@@ -4551,7 +4539,7 @@ implementation_end;
     <Plot>
      <PlotType>GraphPlot</PlotType>
      <BasePlot>
-       <PlotId>5</PlotId>
+       <PlotId>3</PlotId>
        <UseWindowsBGColor>false</UseWindowsBGColor>
        <BGColor>16777215</BGColor>
        <PlotIsVisible>true</PlotIsVisible>
@@ -4697,7 +4685,7 @@ implementation_end;
      <Name>Window 1</Name>
      <Tiling>0</Tiling>
      <PlotIds>
-       <PlotId>4</PlotId>
+       <PlotId>2</PlotId>
      </PlotIds>
      <ToggleState>Base</ToggleState>
     </PlotPanel>
@@ -4707,7 +4695,7 @@ implementation_end;
      <Name>Window 2</Name>
      <Tiling>0</Tiling>
      <PlotIds>
-       <PlotId>3</PlotId>
+       <PlotId>1</PlotId>
      </PlotIds>
      <ToggleState>Base</ToggleState>
     </PlotPanel>
@@ -4717,7 +4705,7 @@ implementation_end;
      <Name>Window 3</Name>
      <Tiling>0</Tiling>
      <PlotIds>
-       <PlotId>5</PlotId>
+       <PlotId>3</PlotId>
      </PlotIds>
      <ToggleState>Base</ToggleState>
     </PlotPanel>