diff --git a/animationR/obj/feet/short/coman_short_foot.mtl b/animationR/obj/feet/short/coman_short_foot.mtl
index 62967a9..78fb0e8 100644
--- a/animationR/obj/feet/short/coman_short_foot.mtl
+++ b/animationR/obj/feet/short/coman_short_foot.mtl
@@ -4,7 +4,7 @@
 newmtl Shape.005
 Ns 500.000000
 Ka 0.200000 0.200000 0.200000
-Kd 1.000000 1.000000 1.000000
+Kd 0.500000 0.500000 0.500000
 Ks 0.050000 0.050000 0.050000
 Ke 0.000000 0.000000 0.000000
 Ni 1.000000
diff --git a/dataR/coman_short_feet.mbs b/dataR/coman_short_feet.mbs
index bcd1a36..5a66dd5 100644
--- a/dataR/coman_short_feet.mbs
+++ b/dataR/coman_short_feet.mbs
@@ -1,4132 +1,4132 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<mbsDescR7>
-  <mbsname>coman_short_feet</mbsname>
-  <mbsyspad_version>1.9.3</mbsyspad_version>
-  <base>
-    <!--this is the base body-->
-    <gravity>
-      <coordinates>
-        <x>0.0</x>
-        <y>0.0</y>
-        <z>-9.81</z>
-      </coordinates>
-    </gravity>
-    <baseBodyName>BaseBody</baseBodyName>
-    <graphics>
-      <x2D>
-        <position>
-          <x>220.0</x>
-          <y>1180.0</y>
-        </position>
-        <base_body>
-          <position>
-            <x>-39.0</x>
-            <y>-82.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>219.0</x>
-            <y>112.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>180.0</x>
-              <y>-82.0</y>
-            </control_point>
-            <control_point>
-              <x>180.0</x>
-              <y>30.0</y>
-            </control_point>
-            <control_point>
-              <x>-39.0</x>
-              <y>30.0</y>
-            </control_point>
-            <control_point>
-              <x>-39.0</x>
-              <y>-82.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </base_body>
-      </x2D>
-      <x3D>
-        <x3D_frame />
-        <x3D_shapes>
-          <box>
-            <name>Floor_ground</name>
-            <position>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>-0.005</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="204" G="204" B="204" />
-            <transparency>0.44999998807907104</transparency>
-            <size>
-              <x>135.0</x>
-              <y>135.0</y>
-              <z>0.01</z>
-            </size>
-          </box>
-          <inline>
-            <name>Floor_grid</name>
-            <position>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>1.0E-4</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>1.5707963267948966</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <url>PRJPATH/animationR/vrml/ground/floor_grid_big.wrl</url>
-          </inline>
-          <box>
-            <name>Border_1</name>
-            <position>
-              <x>0.0</x>
-              <y>-67.5</y>
-              <z>-0.005</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <size>
-              <x>135.0</x>
-              <y>0.1</y>
-              <z>0.01</z>
-            </size>
-          </box>
-          <box>
-            <name>Border_2</name>
-            <position>
-              <x>0.0</x>
-              <y>67.5</y>
-              <z>-0.005</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <size>
-              <x>135.0</x>
-              <y>0.1</y>
-              <z>0.01</z>
-            </size>
-          </box>
-          <box>
-            <name>Border_3</name>
-            <position>
-              <x>-67.5</x>
-              <y>0.0</y>
-              <z>-0.005</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <size>
-              <x>0.1</x>
-              <y>135.0</y>
-              <z>0.01</z>
-            </size>
-          </box>
-          <box>
-            <name>Border_4</name>
-            <position>
-              <x>67.5</x>
-              <y>0.0</y>
-              <z>-0.005</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <size>
-              <x>0.1</x>
-              <y>135.0</y>
-              <z>0.01</z>
-            </size>
-          </box>
-        </x3D_shapes>
-      </x3D>
-    </graphics>
-  </base>
-  <bodytree>
-    <!--this is the body tree-->
-    <body>
-      <bodyname>Waist</bodyname>
-      <parent>
-        <bodyname>base</bodyname>
-        <pointname>origin</pointname>
-      </parent>
-      <joint>
-        <jointname>FJ_T1_Coman</jointname>
-        <type>T1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>264.0</x>
-              <y>1180.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T2_Coman</jointname>
-        <type>T2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>306.0</x>
-              <y>1181.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T3_Coman</jointname>
-        <type>T3</type>
-        <nature>independent</nature>
-        <initialvalue>
-          <q>0.5268</q>
-        </initialvalue>
-        <graphics>
-          <x2D>
-            <position>
-              <x>352.0</x>
-              <y>1181.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R1_Coman</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>317.0</x>
-              <y>770.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R2_Coman</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>304.0</x>
-              <y>627.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R3_Coman</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>299.0</x>
-              <y>472.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.8008695</mass>
-      <com>
-        <coordinates>
-          <x>-0.026391145</x>
-          <y>-5.8216364E-4</y>
-          <z>0.052632312</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0051032982</Ixx>
-        <Ixz>0.0016920543</Ixz>
-        <Iyy>0.006241636</Iyy>
-        <Iyz>1.7225949E-5</Iyz>
-        <Izz>0.0024517762</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="Waist_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_Waist_1</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.023</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>217.0</x>
-              <y>438.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_2</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.023</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>404.0</x>
-              <y>445.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_3</pointname>
-        <coordinates>
-          <x>0.020281504</x>
-          <y>0.0</y>
-          <z>0.11912053</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>306.0</x>
-              <y>395.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_Cg</pointname>
-        <coordinates>
-          <x>-0.026391145</x>
-          <y>-5.8216364E-4</y>
-          <z>0.052632312</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>362.0</x>
-              <y>419.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="WaistCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-83.0</x>
-            <y>-75.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>190.0</x>
-            <y>96.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>107.0</x>
-              <y>-75.0</y>
-            </control_point>
-            <control_point>
-              <x>107.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>-83.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>-83.0</x>
-              <y>-75.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Waist_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_07_prt.wrl</url>
-            </inline>
-            <box>
-              <name>Waist_contact_visu_1</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.1</x>
-                <y>0.2</y>
-                <z>0.11</z>
-              </size>
-            </box>
-            <sphere>
-              <name>Waist_contact_visu_2</name>
-              <position>
-                <x>-0.05</x>
-                <y>0.0</y>
-                <z>0.1</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <radius>0.07</radius>
-            </sphere>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RHipmot</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_1</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>164.0</x>
-              <y>435.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.89258227</mass>
-      <com>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>-0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.7672294E-4</Ixx>
-        <Iyy>5.458038E-4</Iyy>
-        <Izz>5.126859E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RHipmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.0496</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>105.0</x>
-              <y>434.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RHipmot_Cg</pointname>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>-0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>125.0</x>
-              <y>399.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RHipmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-31.0</x>
-            <y>1.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>34.0</x>
-            <y>34.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>-9.0</x>
-              <y>-3.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RHip_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.05</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_20_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RThighUpLeg</bodyname>
-      <parent>
-        <bodyname>RHipmot</bodyname>
-        <pointname>Point_RHipmot</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>97.0</x>
-              <y>506.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0246101</mass>
-      <com>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>-0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.165719E-4</Ixx>
-        <Iyy>0.0010484958</Iyy>
-        <Iyz>2.5492874E-5</Iyz>
-        <Izz>6.8637197E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RThighUpLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1024</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>97.0</x>
-              <y>609.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RThighUpLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>-0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>110.0</x>
-              <y>563.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RThighUpLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-9.0</x>
-            <y>5.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>25.0</x>
-            <y>101.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>28.0</x>
-              <y>-1.0</y>
-            </control_point>
-            <control_point>
-              <x>28.0</x>
-              <y>61.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>61.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>-1.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RThighUp_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_19_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RThighLowLeg</bodyname>
-      <parent>
-        <bodyname>RThighUpLeg</bodyname>
-        <pointname>Point_RThighUpLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>670.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.7001134</mass>
-      <com>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0038979546</Ixx>
-        <Ixy>1.7802399E-6</Ixy>
-        <Ixz>2.6403262E-5</Ixz>
-        <Iyy>0.0039360845</Iyy>
-        <Iyz>4.522711E-5</Iyz>
-        <Izz>9.0218711E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RThighLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1234</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>752.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RThighLowLeg_Cg</pointname>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>109.0</x>
-              <y>719.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RThighLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>83.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RThighLow_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.42</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_18_prt.wrl</url>
-            </inline>
-            <box>
-              <name>RThigh_contact_visu</name>
-              <position>
-                <x>0.003</x>
-                <y>0.005</y>
-                <z>0.01</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.1</x>
-                <y>0.1</y>
-                <z>0.26</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RLowLeg</bodyname>
-      <parent>
-        <bodyname>RThighLowLeg</bodyname>
-        <pointname>Point_RThighLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RKneeSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>91.0</x>
-              <y>816.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.4098179</mass>
-      <com>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>-0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0040604004</Ixx>
-        <Ixy>1.1227206E-5</Ixy>
-        <Ixz>1.2260973E-5</Ixz>
-        <Iyy>0.0040056528</Iyy>
-        <Iyz>5.1899176E-4</Iyz>
-        <Izz>0.0012414054</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RLowLeg_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.201</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>90.0</x>
-              <y>911.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RLowLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>-0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>104.0</x>
-              <y>868.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-13.0</x>
-            <y>3.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>95.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>24.0</x>
-              <y>3.0</y>
-            </control_point>
-            <control_point>
-              <x>24.0</x>
-              <y>91.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>91.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>3.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RLowLeg_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.3</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_17_prt.wrl</url>
-            </inline>
-            <box>
-              <name>RLowLeg_contact_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.005</y>
-                <z>-0.12</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.1</x>
-                <y>0.1</y>
-                <z>0.22</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RFootmot</bodyname>
-      <parent>
-        <bodyname>RLowLeg</bodyname>
-        <pointname>Point_RLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RAnkLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>982.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.72992131</mass>
-      <com>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>-9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.2034425E-4</Ixx>
-        <Ixy>7.6346303E-6</Ixy>
-        <Ixz>2.5430158E-7</Ixz>
-        <Iyy>3.6584358E-4</Iyy>
-        <Izz>3.6065156E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RFootmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>91.0</x>
-              <y>1026.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RFootmot_Cg</pointname>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>-9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>116.0</x>
-              <y>1008.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RFootmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>0.0</x>
-            <y>23.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>25.0</x>
-            <y>25.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>25.0</x>
-              <y>23.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RFootMot_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.1</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_16_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RFoot</bodyname>
-      <parent>
-        <bodyname>RFootmot</bodyname>
-        <pointname>Point_RFootmot</pointname>
-      </parent>
-      <joint>
-        <jointname>RAnkSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>84.0</x>
-              <y>1120.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.489756</mass>
-      <com>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.3467E-4</Ixx>
-        <Iyy>8.0402E-4</Iyy>
-        <Izz>0.0011</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RFoot_Cg</pointname>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>109.0</x>
-              <y>1108.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RFootCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RFoot_F</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>85.0</x>
-              <y>1194.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>-20.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>77.0</x>
-            <y>112.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>41.0</x>
-              <y>-20.0</y>
-            </control_point>
-            <control_point>
-              <x>41.0</x>
-              <y>92.0</y>
-            </control_point>
-            <control_point>
-              <x>-36.0</x>
-              <y>92.0</y>
-            </control_point>
-            <control_point>
-              <x>-36.0</x>
-              <y>-20.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Cache_left</name>
-              <position>
-                <x>0.0</x>
-                <y>0.041</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
-            </inline>
-            <inline>
-              <name>Cache_right</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.036</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
-            </inline>
-            <box>
-              <name>Bar_right</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>-0.06</z>
-              </position>
-              <scale>
-                <x>0.035</x>
-                <y>0.08</y>
-                <z>0.05</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="204" G="204" B="204" />
-              <transparency>0.0</transparency>
-              <size>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </size>
-            </box>
-            <inline>
-              <name>Right_foot</name>
-              <position>
-                <x>-0.06</x>
-                <y>-0.0525</y>
-                <z>-0.0795</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>-1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LHipmot</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_2</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>467.0</x>
-              <y>443.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.89258227</mass>
-      <com>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.7672294E-4</Ixx>
-        <Ixy>3.9936567E-6</Ixy>
-        <Iyy>5.458038E-4</Iyy>
-        <Iyz>3.3389575E-6</Iyz>
-        <Izz>5.126859E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LHipmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0496</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>534.0</x>
-              <y>444.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LHipmot_Cg</pointname>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>503.0</x>
-              <y>409.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LHipmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>36.0</x>
-            <y>0.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>34.0</x>
-            <y>34.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>63.0</x>
-              <y>-10.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LHip_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.04</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_06_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LThighUpLeg</bodyname>
-      <parent>
-        <bodyname>LHipmot</bodyname>
-        <pointname>Point_LHipmot</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>519.0</x>
-              <y>520.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0246101</mass>
-      <com>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.165719E-4</Ixx>
-        <Ixy>1.4449904E-6</Ixy>
-        <Iyy>0.0010484958</Iyy>
-        <Izz>6.8637197E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LThighUpLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1024</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>519.0</x>
-              <y>623.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LThighUpLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>488.0</x>
-              <y>578.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LThighUpLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>7.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>24.0</x>
-            <y>101.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>12.0</x>
-              <y>7.0</y>
-            </control_point>
-            <control_point>
-              <x>12.0</x>
-              <y>108.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>108.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>7.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LThighUp_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_05_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LThighLowLeg</bodyname>
-      <parent>
-        <bodyname>LThighUpLeg</bodyname>
-        <pointname>Point_LThighUpLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>521.0</x>
-              <y>680.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.7001134</mass>
-      <com>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>-6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0038979546</Ixx>
-        <Ixz>2.6403262E-5</Ixz>
-        <Iyy>0.0039360845</Iyy>
-        <Izz>9.0218711E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LThighLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1234</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>523.0</x>
-              <y>759.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LThighLowLeg_Cg</pointname>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>-6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>509.0</x>
-              <y>730.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LThighLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>25.0</x>
-            <y>82.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>13.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>86.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>86.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LTighLow_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.078</y>
-                <z>-0.42</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_04_prt.wrl</url>
-            </inline>
-            <box>
-              <name>LThigh_contact_visu</name>
-              <position>
-                <x>0.003</x>
-                <y>-0.005</y>
-                <z>0.01</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.1</x>
-                <y>0.1</y>
-                <z>0.26</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LLowLeg</bodyname>
-      <parent>
-        <bodyname>LThighLowLeg</bodyname>
-        <pointname>Point_LThighLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LKneeSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>516.0</x>
-              <y>824.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.4098179</mass>
-      <com>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0040604004</Ixx>
-        <Ixz>1.2260973E-5</Ixz>
-        <Iyy>0.0040056528</Iyy>
-        <Izz>0.0012414054</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LLowLeg_contact_visu" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.201</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>517.0</x>
-              <y>916.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LLowLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>493.0</x>
-              <y>890.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-15.0</x>
-            <y>10.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>88.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>29.0</x>
-              <y>-17.0</y>
-            </control_point>
-            <control_point>
-              <x>29.0</x>
-              <y>52.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>52.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>-17.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LLowLeg_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.3</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_03_prt.wrl</url>
-            </inline>
-            <box>
-              <name>LLowLeg_contact_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.005</y>
-                <z>-0.12</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.1</x>
-                <y>0.1</y>
-                <z>0.22</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LFootmot</bodyname>
-      <parent>
-        <bodyname>LLowLeg</bodyname>
-        <pointname>Point_LLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LAnkLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>512.0</x>
-              <y>985.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.72992131</mass>
-      <com>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.2034425E-4</Ixx>
-        <Ixz>2.5430158E-7</Ixz>
-        <Iyy>3.6584358E-4</Iyy>
-        <Iyz>3.6889276E-6</Iyz>
-        <Izz>3.6065156E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LFootmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>509.0</x>
-              <y>1030.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LFootmot_Cg</pointname>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>488.0</x>
-              <y>1009.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LFootmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-4.0</x>
-            <y>23.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>21.0</x>
-            <y>21.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>39.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LFootMot_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.1</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_02_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LFoot</bodyname>
-      <parent>
-        <bodyname>LFootmot</bodyname>
-        <pointname>Point_LFootmot</pointname>
-      </parent>
-      <joint>
-        <jointname>LAnkSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>495.0</x>
-              <y>1148.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.489756</mass>
-      <com>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.3467E-4</Ixx>
-        <Iyy>8.0402E-4</Iyy>
-        <Izz>0.0011</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LFoot_Cg</pointname>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>522.0</x>
-              <y>1144.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LFootCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LFoot_F</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>495.0</x>
-              <y>1221.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-37.0</x>
-            <y>-19.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>72.0</x>
-            <y>108.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>35.0</x>
-              <y>-19.0</y>
-            </control_point>
-            <control_point>
-              <x>35.0</x>
-              <y>89.0</y>
-            </control_point>
-            <control_point>
-              <x>-37.0</x>
-              <y>89.0</y>
-            </control_point>
-            <control_point>
-              <x>-37.0</x>
-              <y>-19.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Cache_left</name>
-              <position>
-                <x>0.0</x>
-                <y>0.041</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
-            </inline>
-            <inline>
-              <name>Cache_right</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.037</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
-            </inline>
-            <box>
-              <name>Bar_left</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.0</y>
-                <z>-0.06</z>
-              </position>
-              <scale>
-                <x>0.035</x>
-                <y>0.08</y>
-                <z>0.05</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="204" G="204" B="204" />
-              <transparency>0.0</transparency>
-              <size>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </size>
-            </box>
-            <inline>
-              <name>Left_foot</name>
-              <position>
-                <x>-0.06</x>
-                <y>-0.0525</y>
-                <z>-0.0795</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>-1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWL</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_3</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>305.0</x>
-              <y>331.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.75398402</mass>
-      <com>
-        <coordinates>
-          <x>-0.0021273416</x>
-          <y>0.0033575235</y>
-          <z>-0.0010076896</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.4093348E-4</Ixx>
-        <Iyy>3.9581916E-4</Iyy>
-        <Iyz>1.443019E-6</Iyz>
-        <Izz>3.7449753E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_DWL</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>304.0</x>
-              <y>294.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWL_Cg</pointname>
-        <coordinates>
-          <x>-0.0021273416</x>
-          <y>0.0033575235</y>
-          <z>-0.0010076896</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>336.0</x>
-              <y>313.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWLCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-52.0</x>
-            <y>-44.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>103.0</x>
-            <y>55.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>21.0</x>
-              <y>-51.0</y>
-            </control_point>
-            <control_point>
-              <x>21.0</x>
-              <y>-3.0</y>
-            </control_point>
-            <control_point>
-              <x>1.0</x>
-              <y>-3.0</y>
-            </control_point>
-            <control_point>
-              <x>1.0</x>
-              <y>-51.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>DWL1_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.0</y>
-                <z>-0.64</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_09_prt.wrl</url>
-            </inline>
-            <inline>
-              <name>DWL2_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.0</y>
-                <z>-0.64</z>
-              </position>
-              <scale>
-                <x>0.039</x>
-                <y>0.039</y>
-                <z>0.039</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_08_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWS</bodyname>
-      <parent>
-        <bodyname>DWL</bodyname>
-        <pointname>Point_DWL</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>310.0</x>
-              <y>245.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.54588774</mass>
-      <com>
-        <coordinates>
-          <x>4.2999707E-4</x>
-          <y>-0.0097650317</y>
-          <z>0.033011157</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.7126578E-4</Ixx>
-        <Ixz>1.1440119E-6</Ixz>
-        <Iyy>7.4686706E-4</Iyy>
-        <Izz>6.2196235E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_DWS</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0475</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>308.0</x>
-              <y>192.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWS_Cg</pointname>
-        <coordinates>
-          <x>4.2999707E-4</x>
-          <y>-0.0097650317</y>
-          <z>0.033011157</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>336.0</x>
-              <y>223.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWSCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-35.0</x>
-            <y>-47.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>65.0</x>
-            <y>47.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>0.0</x>
-              <y>-18.0</y>
-            </control_point>
-            <control_point>
-              <x>0.0</x>
-              <y>-6.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-6.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-18.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D />
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWYTorso</bodyname>
-      <parent>
-        <bodyname>DWS</bodyname>
-        <pointname>Point_DWS</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>307.0</x>
-              <y>144.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>6.3617175</mass>
-      <com>
-        <coordinates>
-          <x>-0.03585012</x>
-          <y>1.8286043E-5</y>
-          <z>0.117557148</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.034089342</Ixx>
-        <Iyy>0.030117357</Iyy>
-        <Iyz>6.2443771E-6</Iyz>
-        <Izz>0.025296582</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="Torso_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_Cg</pointname>
-        <coordinates>
-          <x>-0.03585012</x>
-          <y>1.8286043E-5</y>
-          <z>0.117557148</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>350.0</x>
-              <y>97.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWYCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_1</pointname>
-        <coordinates>
-          <x>-0.015</x>
-          <y>-0.0825</y>
-          <z>0.1554</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>252.0</x>
-              <y>114.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_2</pointname>
-        <coordinates>
-          <x>-0.015</x>
-          <y>0.0825</y>
-          <z>0.1554</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>375.0</x>
-              <y>114.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-62.0</x>
-            <y>-51.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>138.0</x>
-            <y>61.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>-1.0</x>
-              <y>-8.0</y>
-            </control_point>
-            <control_point>
-              <x>-1.0</x>
-              <y>-5.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-5.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-8.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Torso_visu</name>
-              <position>
-                <x>-0.035</x>
-                <y>0.0</y>
-                <z>-0.69</z>
-              </position>
-              <scale>
-                <x>0.0393</x>
-                <y>0.0393</y>
-                <z>0.0393</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/torso_blue.wrl</url>
-            </inline>
-            <sphere>
-              <name>Torso_contact_visu_1</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>-0.05</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <radius>0.06</radius>
-            </sphere>
-            <box>
-              <name>Torso_contact_visu_2</name>
-              <position>
-                <x>-0.04</x>
-                <y>0.0</y>
-                <z>0.17</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.12</x>
-                <y>0.35</y>
-                <z>0.1</z>
-              </size>
-            </box>
-            <box>
-              <name>Torso_contact_visu_3</name>
-              <position>
-                <x>-0.05</x>
-                <y>0.0</y>
-                <z>0.13</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.19</x>
-                <y>0.18</y>
-                <z>0.2</z>
-              </size>
-            </box>
-            <sphere>
-              <name>Torso_contact_visu_4</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.0</y>
-                <z>0.02</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <radius>0.07</radius>
-            </sphere>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShp</bodyname>
-      <parent>
-        <bodyname>DWYTorso</bodyname>
-        <pointname>Point_DWYTorso_1</pointname>
-      </parent>
-      <joint>
-        <jointname>RShSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>217.0</x>
-              <y>105.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.56782202</mass>
-      <com>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>-0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>7.8337456E-4</Ixx>
-        <Ixy>1.9035768E-4</Ixy>
-        <Iyy>6.3399921E-4</Iyy>
-        <Iyz>4.3840774E-6</Iyz>
-        <Izz>0.001008054</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RShp_Cg</pointname>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>-0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>162.0</x>
-              <y>99.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShpCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShp</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.071</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>156.0</x>
-              <y>129.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>7.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>30.0</x>
-            <y>30.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>3.0</x>
-              <y>12.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShp_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.08</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_24_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShr</bodyname>
-      <parent>
-        <bodyname>RShp</bodyname>
-        <pointname>Point_RShp</pointname>
-      </parent>
-      <joint>
-        <jointname>RShLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>114.0</x>
-              <y>123.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.77683752</mass>
-      <com>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.5812904E-4</Ixx>
-        <Ixz>9.2521185E-6</Ixz>
-        <Iyy>4.3089204E-4</Iyy>
-        <Izz>3.4174855E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RShr_Cg</pointname>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>80.0</x>
-              <y>141.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShrCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShr</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.0425</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>105.0</x>
-              <y>170.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>5.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>56.0</x>
-            <y>43.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>65.0</y>
-            </control_point>
-            <control_point>
-              <x>-22.0</x>
-              <y>65.0</y>
-            </control_point>
-            <control_point>
-              <x>-22.0</x>
-              <y>21.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShr_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_23_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShy</bodyname>
-      <parent>
-        <bodyname>RShr</bodyname>
-        <pointname>Point_RShr</pointname>
-      </parent>
-      <joint>
-        <jointname>RShYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>123.0</x>
-              <y>226.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0462304</mass>
-      <com>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>-0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0030934574</Ixx>
-        <Ixy>6.6702837E-5</Ixy>
-        <Ixz>3.0222291E-4</Ixz>
-        <Iyy>0.0029181573</Iyy>
-        <Izz>8.0830389E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShy_Cg</pointname>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>-0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>107.0</x>
-              <y>258.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShyCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShy</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1375</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>124.0</x>
-              <y>289.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-17.0</x>
-            <y>-1.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>32.0</x>
-            <y>66.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>31.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>31.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>3.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>3.0</x>
-              <y>21.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShy_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.8</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_22_prt.wrl</url>
-            </inline>
-            <box>
-              <name>Rhoulder_contact_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.015</y>
-                <z>-0.05</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.08</x>
-                <y>0.08</y>
-                <z>0.27</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RElb</bodyname>
-      <parent>
-        <bodyname>RShy</bodyname>
-        <pointname>Point_RShy</pointname>
-      </parent>
-      <joint>
-        <jointname>RElb</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>161.0</x>
-              <y>318.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.63306933</mass>
-      <com>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>-9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>2.2599075E-4</Ixx>
-        <Iyy>2.7781302E-4</Iyy>
-        <Izz>2.1878992E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RElb_Cg</pointname>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>-9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>157.0</x>
-              <y>352.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RElbCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-17.0</x>
-            <y>6.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>30.0</x>
-            <y>46.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>18.0</x>
-              <y>26.0</y>
-            </control_point>
-            <control_point>
-              <x>18.0</x>
-              <y>63.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>63.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>26.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RElb_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.67</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_21_prt.wrl</url>
-            </inline>
-            <box>
-              <name>RElbow_contact_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.005</y>
-                <z>-0.13</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.032</x>
-                <y>0.032</y>
-                <z>0.16</z>
-              </size>
-            </box>
-            <sphere>
-              <name>RWrist_contact_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.005</y>
-                <z>-0.208</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <radius>0.021</radius>
-            </sphere>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShp</bodyname>
-      <parent>
-        <bodyname>DWYTorso</bodyname>
-        <pointname>Point_DWYTorso_2</pointname>
-      </parent>
-      <joint>
-        <jointname>LShSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>423.0</x>
-              <y>107.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.56782202</mass>
-      <com>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>7.8337456E-4</Ixx>
-        <Iyy>6.3399921E-4</Iyy>
-        <Izz>0.001008054</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LShp_Cg</pointname>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>474.0</x>
-              <y>95.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShpCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShp</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.071</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>481.0</x>
-              <y>125.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>35.0</x>
-            <y>9.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>31.0</x>
-            <y>31.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>64.0</x>
-              <y>13.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShp_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.08</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_14_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShr</bodyname>
-      <parent>
-        <bodyname>LShp</bodyname>
-        <pointname>Point_LShp</pointname>
-      </parent>
-      <joint>
-        <jointname>LShLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>531.0</x>
-              <y>127.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.77683752</mass>
-      <com>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>-2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.5812904E-4</Ixx>
-        <Ixy>2.4111086E-7</Ixy>
-        <Ixz>9.2521185E-6</Ixz>
-        <Iyy>4.3089204E-4</Iyy>
-        <Iyz>4.721695E-7</Iyz>
-        <Izz>3.4174855E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LShr_Cg</pointname>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>-2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>560.0</x>
-              <y>142.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShrCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShr</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.0425</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>541.0</x>
-              <y>165.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-26.0</x>
-            <y>2.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>57.0</x>
-            <y>39.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>68.0</x>
-              <y>24.0</y>
-            </control_point>
-            <control_point>
-              <x>68.0</x>
-              <y>60.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>60.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>24.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShr_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_13_prt.wrl</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShy</bodyname>
-      <parent>
-        <bodyname>LShr</bodyname>
-        <pointname>Point_LShr</pointname>
-      </parent>
-      <joint>
-        <jointname>LShYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>573.0</x>
-              <y>212.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0462304</mass>
-      <com>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0030934574</Ixx>
-        <Ixz>3.0222291E-4</Ixz>
-        <Iyy>0.0029181573</Iyy>
-        <Iyz>5.2663661E-4</Iyz>
-        <Izz>8.0830389E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShy_Cg</pointname>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>590.0</x>
-              <y>248.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShyCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShy</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1375</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>579.0</x>
-              <y>284.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-15.0</x>
-            <y>3.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>31.0</x>
-            <y>71.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>18.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>18.0</x>
-              <y>67.0</y>
-            </control_point>
-            <control_point>
-              <x>-8.0</x>
-              <y>67.0</y>
-            </control_point>
-            <control_point>
-              <x>-8.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShy_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.8</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_12_prt.wrl</url>
-            </inline>
-            <box>
-              <name>LSoulder_contact_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.015</y>
-                <z>-0.05</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.08</x>
-                <y>0.08</y>
-                <z>0.27</z>
-              </size>
-            </box>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LElb</bodyname>
-      <parent>
-        <bodyname>LShy</bodyname>
-        <pointname>Point_LShy</pointname>
-      </parent>
-      <joint>
-        <jointname>LElb</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>604.0</x>
-              <y>318.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.63306933</mass>
-      <com>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>2.2599075E-4</Ixx>
-        <Ixy>2.637647E-7</Ixy>
-        <Iyy>2.7781302E-4</Iyy>
-        <Iyz>2.0435288E-6</Iyz>
-        <Izz>2.1878992E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LElb_Cg</pointname>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>623.0</x>
-              <y>345.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LElbCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>31.0</x>
-            <y>46.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>19.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>19.0</x>
-              <y>50.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>50.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LElb_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.67</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_11_prt.wrl</url>
-            </inline>
-            <box>
-              <name>LElbow_contact_visu</name>
-              <position>
-                <x>-0.015</x>
-                <y>0.005</y>
-                <z>-0.13</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <size>
-                <x>0.032</x>
-                <y>0.032</y>
-                <z>0.16</z>
-              </size>
-            </box>
-            <sphere>
-              <name>Lwrist_contact_visu</name>
-              <position>
-                <x>-0.015</x>
-                <y>0.005</y>
-                <z>-0.208</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="102" B="0" />
-              <transparency>1.0</transparency>
-              <radius>0.021</radius>
-            </sphere>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-  </bodytree>
-  <user_models>
-    <!--this is the userModel list-->
-    <user_model>
-      <name>cpp</name>
-      <parameter>
-        <name>interface</name>
-        <type>structure</type>
-        <structure>void</structure>
-        <header />
-      </parameter>
-    </user_model>
-  </user_models>
-  <x3D>
-    <background>
-      <!--This is the background properties-->
-      <color R="255" G="255" B="255" />
-    </background>
-    <shapeScaleFactors>
-      <!--This is the scale factors-->
-      <frame>0.2</frame>
-      <joint>0.3</joint>
-      <com>0.3</com>
-      <point>0.3</point>
-    </shapeScaleFactors>
-    <viewpoints>
-      <!--This is the viewpoint list-->
-      <viewpoint>
-        <name>Robot</name>
-        <joint>FJ_T3_Coman</joint>
-        <position>
-          <x>1.0</x>
-          <y>1.0</y>
-          <z>0.5</z>
-        </position>
-        <target>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </target>
-        <up>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>1.0</z>
-        </up>
-        <projection>parallel</projection>
-        <scale>0.2</scale>
-        <fieldOfView>2.0</fieldOfView>
-        <frontClipDst>0.01</frontClipDst>
-        <backClipDst>50.0</backClipDst>
-      </viewpoint>
-      <viewpoint>
-        <name>Profile</name>
-        <joint>FJ_T2_Coman</joint>
-        <position>
-          <x>0.0</x>
-          <y>-1.0</y>
-          <z>0.5</z>
-        </position>
-        <target>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.5</z>
-        </target>
-        <up>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>1.0</z>
-        </up>
-        <projection>parallel</projection>
-        <scale>0.3</scale>
-        <fieldOfView>0.785</fieldOfView>
-        <frontClipDst>0.01</frontClipDst>
-        <backClipDst>50.0</backClipDst>
-      </viewpoint>
-    </viewpoints>
-    <lights>
-      <!--This is the light list-->
-      <spotLight>
-        <name>X_fwd</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>-10.0</x>
-          <y>-10.0</y>
-          <z>0.0</z>
-        </position>
-        <attenuation>
-          <cst>1.5</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-        <direction>
-          <x>1.0</x>
-          <y>1.0</y>
-          <z>0.0</z>
-        </direction>
-        <angle>3.1415927</angle>
-        <concentration>0.0</concentration>
-      </spotLight>
-      <spotLight>
-        <name>X_bwd</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>10.0</x>
-          <y>10.0</y>
-          <z>0.0</z>
-        </position>
-        <attenuation>
-          <cst>1.5</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-        <direction>
-          <x>-1.0</x>
-          <y>-1.0</y>
-          <z>0.0</z>
-        </direction>
-        <angle>3.1415927</angle>
-        <concentration>0.0</concentration>
-      </spotLight>
-      <spotLight>
-        <name>Z</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>10.0</z>
-        </position>
-        <attenuation>
-          <cst>1.2000000476837158</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-        <direction>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-1.0</z>
-        </direction>
-        <angle>3.1415927</angle>
-        <concentration>0.0</concentration>
-      </spotLight>
-    </lights>
-  </x3D>
-</mbsDescR7>
-
+<?xml version="1.0" encoding="UTF-8"?>
+<mbsDescR7>
+  <mbsname>coman_short_feet</mbsname>
+  <mbsyspad_version>1.9.4</mbsyspad_version>
+  <base>
+    <!--this is the base body-->
+    <gravity>
+      <coordinates>
+        <x>0.0</x>
+        <y>0.0</y>
+        <z>-9.81</z>
+      </coordinates>
+    </gravity>
+    <baseBodyName>BaseBody</baseBodyName>
+    <graphics>
+      <x2D>
+        <position>
+          <x>220.0</x>
+          <y>1180.0</y>
+        </position>
+        <base_body>
+          <position>
+            <x>-39.0</x>
+            <y>-82.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>219.0</x>
+            <y>112.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>180.0</x>
+              <y>-82.0</y>
+            </control_point>
+            <control_point>
+              <x>180.0</x>
+              <y>30.0</y>
+            </control_point>
+            <control_point>
+              <x>-39.0</x>
+              <y>30.0</y>
+            </control_point>
+            <control_point>
+              <x>-39.0</x>
+              <y>-82.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </base_body>
+      </x2D>
+      <x3D>
+        <x3D_frame />
+        <x3D_shapes>
+          <box>
+            <name>Floor_ground</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="204" G="204" B="204" />
+            <transparency>0.44999998807907104</transparency>
+            <size>
+              <x>135.0</x>
+              <y>135.0</y>
+              <z>0.01</z>
+            </size>
+          </box>
+          <inline>
+            <name>Floor_grid</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>1.0E-4</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>1.5707963267948966</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <url>PRJPATH/animationR/vrml/ground/floor_grid_big.wrl</url>
+          </inline>
+          <box>
+            <name>Border_1</name>
+            <position>
+              <x>0.0</x>
+              <y>-67.5</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <size>
+              <x>135.0</x>
+              <y>0.1</y>
+              <z>0.01</z>
+            </size>
+          </box>
+          <box>
+            <name>Border_2</name>
+            <position>
+              <x>0.0</x>
+              <y>67.5</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <size>
+              <x>135.0</x>
+              <y>0.1</y>
+              <z>0.01</z>
+            </size>
+          </box>
+          <box>
+            <name>Border_3</name>
+            <position>
+              <x>-67.5</x>
+              <y>0.0</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <size>
+              <x>0.1</x>
+              <y>135.0</y>
+              <z>0.01</z>
+            </size>
+          </box>
+          <box>
+            <name>Border_4</name>
+            <position>
+              <x>67.5</x>
+              <y>0.0</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <size>
+              <x>0.1</x>
+              <y>135.0</y>
+              <z>0.01</z>
+            </size>
+          </box>
+        </x3D_shapes>
+      </x3D>
+    </graphics>
+  </base>
+  <bodytree>
+    <!--this is the body tree-->
+    <body>
+      <bodyname>Waist</bodyname>
+      <parent>
+        <bodyname>base</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_T1_Coman</jointname>
+        <type>T1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>264.0</x>
+              <y>1180.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T2_Coman</jointname>
+        <type>T2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T3_Coman</jointname>
+        <type>T3</type>
+        <nature>independent</nature>
+        <initialvalue>
+          <q>0.5268</q>
+        </initialvalue>
+        <graphics>
+          <x2D>
+            <position>
+              <x>352.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R1_Coman</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>317.0</x>
+              <y>770.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_Coman</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>627.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R3_Coman</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>299.0</x>
+              <y>472.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.8008695</mass>
+      <com>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0051032982</Ixx>
+        <Ixz>0.0016920543</Ixz>
+        <Iyy>0.006241636</Iyy>
+        <Iyz>1.7225949E-5</Iyz>
+        <Izz>0.0024517762</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Waist_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_Waist_1</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>438.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_2</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>404.0</x>
+              <y>445.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_3</pointname>
+        <coordinates>
+          <x>0.020281504</x>
+          <y>0.0</y>
+          <z>0.11912053</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>395.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_Cg</pointname>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>362.0</x>
+              <y>419.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="WaistCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-83.0</x>
+            <y>-75.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>190.0</x>
+            <y>96.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>107.0</x>
+              <y>-75.0</y>
+            </control_point>
+            <control_point>
+              <x>107.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>-75.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Waist_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_07_prt.wrl</url>
+            </inline>
+            <box>
+              <name>Waist_contact_visu_1</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.1</x>
+                <y>0.2</y>
+                <z>0.11</z>
+              </size>
+            </box>
+            <sphere>
+              <name>Waist_contact_visu_2</name>
+              <position>
+                <x>-0.05</x>
+                <y>0.0</y>
+                <z>0.1</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <radius>0.07</radius>
+            </sphere>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>164.0</x>
+              <y>435.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Iyy>5.458038E-4</Iyy>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>434.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>125.0</x>
+              <y>399.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-31.0</x>
+            <y>1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-9.0</x>
+              <y>-3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.05</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_20_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighUpLeg</bodyname>
+      <parent>
+        <bodyname>RHipmot</bodyname>
+        <pointname>Point_RHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>506.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Iyy>0.0010484958</Iyy>
+        <Iyz>2.5492874E-5</Iyz>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>609.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>110.0</x>
+              <y>563.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-9.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>28.0</x>
+              <y>-1.0</y>
+            </control_point>
+            <control_point>
+              <x>28.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>-1.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_19_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighUpLeg</bodyname>
+        <pointname>Point_RThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>670.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixy>1.7802399E-6</Ixy>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Iyz>4.522711E-5</Iyz>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>752.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>719.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>83.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_18_prt.wrl</url>
+            </inline>
+            <box>
+              <name>RThigh_contact_visu</name>
+              <position>
+                <x>0.003</x>
+                <y>0.005</y>
+                <z>0.01</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.1</x>
+                <y>0.1</y>
+                <z>0.26</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighLowLeg</bodyname>
+        <pointname>Point_RThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>816.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixy>1.1227206E-5</Ixy>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Iyz>5.1899176E-4</Iyz>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RLowLeg_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>90.0</x>
+              <y>911.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>104.0</x>
+              <y>868.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-13.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>95.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>24.0</x>
+              <y>3.0</y>
+            </control_point>
+            <control_point>
+              <x>24.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_17_prt.wrl</url>
+            </inline>
+            <box>
+              <name>RLowLeg_contact_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.005</y>
+                <z>-0.12</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.1</x>
+                <y>0.1</y>
+                <z>0.22</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFootmot</bodyname>
+      <parent>
+        <bodyname>RLowLeg</bodyname>
+        <pointname>Point_RLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>982.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixy>7.6346303E-6</Ixy>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>1026.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>116.0</x>
+              <y>1008.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>0.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>25.0</x>
+            <y>25.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>25.0</x>
+              <y>23.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_16_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFoot</bodyname>
+      <parent>
+        <bodyname>RFootmot</bodyname>
+        <pointname>Point_RFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>84.0</x>
+              <y>1120.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>1108.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>85.0</x>
+              <y>1194.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>-20.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>77.0</x>
+            <y>112.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>41.0</x>
+              <y>-20.0</y>
+            </control_point>
+            <control_point>
+              <x>41.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>-20.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.036</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <box>
+              <name>Bar_right</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.08</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="204" G="204" B="204" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Right_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>467.0</x>
+              <y>443.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Ixy>3.9936567E-6</Ixy>
+        <Iyy>5.458038E-4</Iyy>
+        <Iyz>3.3389575E-6</Iyz>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>534.0</x>
+              <y>444.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>503.0</x>
+              <y>409.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>36.0</x>
+            <y>0.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>63.0</x>
+              <y>-10.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.04</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_06_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighUpLeg</bodyname>
+      <parent>
+        <bodyname>LHipmot</bodyname>
+        <pointname>Point_LHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>520.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Ixy>1.4449904E-6</Ixy>
+        <Iyy>0.0010484958</Iyy>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>623.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>578.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>24.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>12.0</x>
+              <y>7.0</y>
+            </control_point>
+            <control_point>
+              <x>12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>7.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_05_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighUpLeg</bodyname>
+        <pointname>Point_LThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>521.0</x>
+              <y>680.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>523.0</x>
+              <y>759.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>730.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>82.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>13.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LTighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.078</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_04_prt.wrl</url>
+            </inline>
+            <box>
+              <name>LThigh_contact_visu</name>
+              <position>
+                <x>0.003</x>
+                <y>-0.005</y>
+                <z>0.01</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.1</x>
+                <y>0.1</y>
+                <z>0.26</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighLowLeg</bodyname>
+        <pointname>Point_LThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>516.0</x>
+              <y>824.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LLowLeg_contact_visu" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>517.0</x>
+              <y>916.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>493.0</x>
+              <y>890.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>10.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>88.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>29.0</x>
+              <y>-17.0</y>
+            </control_point>
+            <control_point>
+              <x>29.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>-17.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_03_prt.wrl</url>
+            </inline>
+            <box>
+              <name>LLowLeg_contact_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.005</y>
+                <z>-0.12</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.1</x>
+                <y>0.1</y>
+                <z>0.22</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFootmot</bodyname>
+      <parent>
+        <bodyname>LLowLeg</bodyname>
+        <pointname>Point_LLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>512.0</x>
+              <y>985.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Iyz>3.6889276E-6</Iyz>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>1030.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>1009.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-4.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>21.0</x>
+            <y>21.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>39.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_02_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFoot</bodyname>
+      <parent>
+        <bodyname>LFootmot</bodyname>
+        <pointname>Point_LFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1148.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>522.0</x>
+              <y>1144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1221.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-37.0</x>
+            <y>-19.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>72.0</x>
+            <y>108.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>35.0</x>
+              <y>-19.0</y>
+            </control_point>
+            <control_point>
+              <x>35.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>-19.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.037</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <box>
+              <name>Bar_left</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.08</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="204" G="204" B="204" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Left_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWL</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_3</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>305.0</x>
+              <y>331.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.75398402</mass>
+      <com>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.4093348E-4</Ixx>
+        <Iyy>3.9581916E-4</Iyy>
+        <Iyz>1.443019E-6</Iyz>
+        <Izz>3.7449753E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWL</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>294.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWL_Cg</pointname>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>313.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWLCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-52.0</x>
+            <y>-44.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>103.0</x>
+            <y>55.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>21.0</x>
+              <y>-51.0</y>
+            </control_point>
+            <control_point>
+              <x>21.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-51.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>DWL1_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_09_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>DWL2_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.039</x>
+                <y>0.039</y>
+                <z>0.039</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_08_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWS</bodyname>
+      <parent>
+        <bodyname>DWL</bodyname>
+        <pointname>Point_DWL</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>310.0</x>
+              <y>245.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.54588774</mass>
+      <com>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.7126578E-4</Ixx>
+        <Ixz>1.1440119E-6</Ixz>
+        <Iyy>7.4686706E-4</Iyy>
+        <Izz>6.2196235E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWS</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0475</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>308.0</x>
+              <y>192.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWS_Cg</pointname>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>223.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWSCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-35.0</x>
+            <y>-47.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>65.0</x>
+            <y>47.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>0.0</x>
+              <y>-18.0</y>
+            </control_point>
+            <control_point>
+              <x>0.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-18.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWYTorso</bodyname>
+      <parent>
+        <bodyname>DWS</bodyname>
+        <pointname>Point_DWS</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>307.0</x>
+              <y>144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>6.3617175</mass>
+      <com>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.034089342</Ixx>
+        <Iyy>0.030117357</Iyy>
+        <Iyz>6.2443771E-6</Iyz>
+        <Izz>0.025296582</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Torso_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_Cg</pointname>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>350.0</x>
+              <y>97.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWYCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_1</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>-0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>252.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_2</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>375.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-62.0</x>
+            <y>-51.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>138.0</x>
+            <y>61.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-1.0</x>
+              <y>-8.0</y>
+            </control_point>
+            <control_point>
+              <x>-1.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-8.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Torso_visu</name>
+              <position>
+                <x>-0.015</x>
+                <y>0.0</y>
+                <z>-0.69</z>
+              </position>
+              <scale>
+                <x>0.0393</x>
+                <y>0.0393</y>
+                <z>0.0393</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/torso_blue.wrl</url>
+            </inline>
+            <sphere>
+              <name>Torso_contact_visu_1</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.05</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <radius>0.06</radius>
+            </sphere>
+            <box>
+              <name>Torso_contact_visu_2</name>
+              <position>
+                <x>-0.04</x>
+                <y>0.0</y>
+                <z>0.17</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.12</x>
+                <y>0.35</y>
+                <z>0.1</z>
+              </size>
+            </box>
+            <box>
+              <name>Torso_contact_visu_3</name>
+              <position>
+                <x>-0.05</x>
+                <y>0.0</y>
+                <z>0.13</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.19</x>
+                <y>0.18</y>
+                <z>0.2</z>
+              </size>
+            </box>
+            <sphere>
+              <name>Torso_contact_visu_4</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>0.02</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <radius>0.07</radius>
+            </sphere>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>105.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Ixy>1.9035768E-4</Ixy>
+        <Iyy>6.3399921E-4</Iyy>
+        <Iyz>4.3840774E-6</Iyz>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>162.0</x>
+              <y>99.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>156.0</x>
+              <y>129.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>30.0</x>
+            <y>30.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>3.0</x>
+              <y>12.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_24_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShr</bodyname>
+      <parent>
+        <bodyname>RShp</bodyname>
+        <pointname>Point_RShp</pointname>
+      </parent>
+      <joint>
+        <jointname>RShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>114.0</x>
+              <y>123.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>80.0</x>
+              <y>141.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>170.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>56.0</x>
+            <y>43.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_23_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShy</bodyname>
+      <parent>
+        <bodyname>RShr</bodyname>
+        <pointname>Point_RShr</pointname>
+      </parent>
+      <joint>
+        <jointname>RShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>123.0</x>
+              <y>226.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixy>6.6702837E-5</Ixy>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>107.0</x>
+              <y>258.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>124.0</x>
+              <y>289.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>-1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>32.0</x>
+            <y>66.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>31.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>31.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_22_prt.wrl</url>
+            </inline>
+            <box>
+              <name>Rhoulder_contact_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.015</y>
+                <z>-0.05</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.08</x>
+                <y>0.08</y>
+                <z>0.27</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RElb</bodyname>
+      <parent>
+        <bodyname>RShy</bodyname>
+        <pointname>Point_RShy</pointname>
+      </parent>
+      <joint>
+        <jointname>RElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>161.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Iyy>2.7781302E-4</Iyy>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>157.0</x>
+              <y>352.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>6.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>30.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>26.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>26.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_21_prt.wrl</url>
+            </inline>
+            <box>
+              <name>RElbow_contact_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.005</y>
+                <z>-0.13</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.032</x>
+                <y>0.032</y>
+                <z>0.16</z>
+              </size>
+            </box>
+            <sphere>
+              <name>RWrist_contact_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.005</y>
+                <z>-0.208</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <radius>0.021</radius>
+            </sphere>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>423.0</x>
+              <y>107.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Iyy>6.3399921E-4</Iyy>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>474.0</x>
+              <y>95.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>481.0</x>
+              <y>125.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>35.0</x>
+            <y>9.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>31.0</x>
+            <y>31.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>64.0</x>
+              <y>13.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_14_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShr</bodyname>
+      <parent>
+        <bodyname>LShp</bodyname>
+        <pointname>Point_LShp</pointname>
+      </parent>
+      <joint>
+        <jointname>LShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>531.0</x>
+              <y>127.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixy>2.4111086E-7</Ixy>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Iyz>4.721695E-7</Iyz>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>560.0</x>
+              <y>142.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>541.0</x>
+              <y>165.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-26.0</x>
+            <y>2.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>57.0</x>
+            <y>39.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>68.0</x>
+              <y>24.0</y>
+            </control_point>
+            <control_point>
+              <x>68.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>24.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_13_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShy</bodyname>
+      <parent>
+        <bodyname>LShr</bodyname>
+        <pointname>Point_LShr</pointname>
+      </parent>
+      <joint>
+        <jointname>LShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>573.0</x>
+              <y>212.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Iyz>5.2663661E-4</Iyz>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>590.0</x>
+              <y>248.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>579.0</x>
+              <y>284.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>71.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_12_prt.wrl</url>
+            </inline>
+            <box>
+              <name>LSoulder_contact_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.015</y>
+                <z>-0.05</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.08</x>
+                <y>0.08</y>
+                <z>0.27</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LElb</bodyname>
+      <parent>
+        <bodyname>LShy</bodyname>
+        <pointname>Point_LShy</pointname>
+      </parent>
+      <joint>
+        <jointname>LElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>604.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Ixy>2.637647E-7</Ixy>
+        <Iyy>2.7781302E-4</Iyy>
+        <Iyz>2.0435288E-6</Iyz>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>623.0</x>
+              <y>345.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>19.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>19.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_11_prt.wrl</url>
+            </inline>
+            <box>
+              <name>LElbow_contact_visu</name>
+              <position>
+                <x>-0.015</x>
+                <y>0.005</y>
+                <z>-0.13</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <size>
+                <x>0.032</x>
+                <y>0.032</y>
+                <z>0.16</z>
+              </size>
+            </box>
+            <sphere>
+              <name>Lwrist_contact_visu</name>
+              <position>
+                <x>-0.015</x>
+                <y>0.005</y>
+                <z>-0.208</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="102" B="0" />
+              <transparency>1.0</transparency>
+              <radius>0.021</radius>
+            </sphere>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+  </bodytree>
+  <user_models>
+    <!--this is the userModel list-->
+    <user_model>
+      <name>cpp</name>
+      <parameter>
+        <name>interface</name>
+        <type>structure</type>
+        <structure>void</structure>
+        <header />
+      </parameter>
+    </user_model>
+  </user_models>
+  <x3D>
+    <background>
+      <!--This is the background properties-->
+      <color R="255" G="255" B="255" />
+    </background>
+    <shapeScaleFactors>
+      <!--This is the scale factors-->
+      <frame>0.2</frame>
+      <joint>0.3</joint>
+      <com>0.3</com>
+      <point>0.3</point>
+    </shapeScaleFactors>
+    <viewpoints>
+      <!--This is the viewpoint list-->
+      <viewpoint>
+        <name>Robot</name>
+        <joint>FJ_T3_Coman</joint>
+        <position>
+          <x>1.0</x>
+          <y>1.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>parallel</projection>
+        <scale>0.2</scale>
+        <fieldOfView>2.0</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+      <viewpoint>
+        <name>Profile</name>
+        <joint>FJ_T2_Coman</joint>
+        <position>
+          <x>0.0</x>
+          <y>-1.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.5</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>parallel</projection>
+        <scale>0.3</scale>
+        <fieldOfView>0.785</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+    </viewpoints>
+    <lights>
+      <!--This is the light list-->
+      <spotLight>
+        <name>X_fwd</name>
+        <joint>FJ_T3_Coman</joint>
+        <active />
+        <position>
+          <x>-10.0</x>
+          <y>-10.0</y>
+          <z>0.0</z>
+        </position>
+        <attenuation>
+          <cst>1.5</cst>
+          <lin>0.0</lin>
+          <quad>0.0</quad>
+        </attenuation>
+        <direction>
+          <x>1.0</x>
+          <y>1.0</y>
+          <z>0.0</z>
+        </direction>
+        <angle>3.1415927</angle>
+        <concentration>0.0</concentration>
+      </spotLight>
+      <spotLight>
+        <name>X_bwd</name>
+        <joint>FJ_T3_Coman</joint>
+        <active />
+        <position>
+          <x>10.0</x>
+          <y>10.0</y>
+          <z>0.0</z>
+        </position>
+        <attenuation>
+          <cst>1.5</cst>
+          <lin>0.0</lin>
+          <quad>0.0</quad>
+        </attenuation>
+        <direction>
+          <x>-1.0</x>
+          <y>-1.0</y>
+          <z>0.0</z>
+        </direction>
+        <angle>3.1415927</angle>
+        <concentration>0.0</concentration>
+      </spotLight>
+      <spotLight>
+        <name>Z</name>
+        <joint>FJ_T3_Coman</joint>
+        <active />
+        <position>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>10.0</z>
+        </position>
+        <attenuation>
+          <cst>1.2000000476837158</cst>
+          <lin>0.0</lin>
+          <quad>0.0</quad>
+        </attenuation>
+        <direction>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-1.0</z>
+        </direction>
+        <angle>3.1415927</angle>
+        <concentration>0.0</concentration>
+      </spotLight>
+    </lights>
+  </x3D>
+</mbsDescR7>
+
diff --git a/dataR/coman_short_feet_simp.mbs b/dataR/coman_short_feet_simp.mbs
new file mode 100644
index 0000000..cedec26
--- /dev/null
+++ b/dataR/coman_short_feet_simp.mbs
@@ -0,0 +1,1791 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<mbsDescR7>
+  <mbsname>coman_short_feet_simp</mbsname>
+  <mbsyspad_version>1.9.4</mbsyspad_version>
+  <base>
+    <!--this is the base body-->
+    <gravity>
+      <coordinates>
+        <x>0.0</x>
+        <y>0.0</y>
+        <z>-9.81</z>
+      </coordinates>
+    </gravity>
+    <baseBodyName>Body_0</baseBodyName>
+    <graphics>
+      <x2D>
+        <position>
+          <x>221.0</x>
+          <y>729.0</y>
+        </position>
+        <base_body>
+          <position>
+            <x>-60.0</x>
+            <y>-64.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>188.0</x>
+            <y>100.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>128.0</x>
+              <y>-64.0</y>
+            </control_point>
+            <control_point>
+              <x>128.0</x>
+              <y>36.0</y>
+            </control_point>
+            <control_point>
+              <x>-60.0</x>
+              <y>36.0</y>
+            </control_point>
+            <control_point>
+              <x>-60.0</x>
+              <y>-64.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </base_body>
+      </x2D>
+      <x3D>
+        <x3D_frame />
+        <x3D_shapes>
+          <box>
+            <name>Floor_ground</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>-0.005</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="102" G="102" B="102" />
+            <transparency>0.0</transparency>
+            <size>
+              <x>135.0</x>
+              <y>135.0</y>
+              <z>0.01</z>
+            </size>
+          </box>
+          <inline>
+            <name>Floor_grid</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>1.0E-4</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>1.5707963267948966</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <url>PRJPATH/animationR/vrml/ground/floor_grid_big.wrl</url>
+          </inline>
+        </x3D_shapes>
+      </x3D>
+    </graphics>
+  </base>
+  <bodytree>
+    <!--this is the body tree-->
+    <body>
+      <bodyname>Waist</bodyname>
+      <parent>
+        <bodyname>base</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_T1_Coman</jointname>
+        <type>T1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>241.0</x>
+              <y>617.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T3_Coman</jointname>
+        <type>T3</type>
+        <nature>independent</nature>
+        <initialvalue>
+          <q>0.5268</q>
+        </initialvalue>
+        <graphics>
+          <x2D>
+            <position>
+              <x>249.0</x>
+              <y>506.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_Coman</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>250.0</x>
+              <y>412.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>2.55485</mass>
+      <com>
+        <coordinates>
+          <x>-0.013245</x>
+          <y>5.8051E-4</y>
+          <z>0.0719568</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.00773126</Ixx>
+        <Ixy>-9.327E-5</Ixy>
+        <Ixz>1.41837E-4</Ixz>
+        <Iyy>0.00997082</Iyy>
+        <Iyz>-1.18436E-4</Iyz>
+        <Izz>0.00388911</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>177.0</x>
+              <y>393.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_1</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>317.0</x>
+              <y>392.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_2</pointname>
+        <coordinates>
+          <x>0.020281504</x>
+          <y>0.0</y>
+          <z>0.11912053</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>251.0</x>
+              <y>352.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-77.0</x>
+            <y>-68.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>147.0</x>
+            <y>91.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>70.0</x>
+              <y>-68.0</y>
+            </control_point>
+            <control_point>
+              <x>70.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-77.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-77.0</x>
+              <y>-68.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Waist_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_07_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>DWL1_visu</name>
+              <position>
+                <x>2.81504E-4</x>
+                <y>0.0</y>
+                <z>-0.52087947</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_09_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>DWL2_visu</name>
+              <position>
+                <x>2.81504E-4</x>
+                <y>0.0</y>
+                <z>-0.52087947</z>
+              </position>
+              <scale>
+                <x>0.039</x>
+                <y>0.039</y>
+                <z>0.039</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_08_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThigh</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>393.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>3.61731</mass>
+      <com>
+        <coordinates>
+          <x>7.39959E-4</x>
+          <y>-0.0476521</y>
+          <z>-0.100464</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0271576</Ixx>
+        <Ixy>3.52594E-5</Ixy>
+        <Ixz>1.32356E-4</Ixz>
+        <Iyy>0.0265146</Iyy>
+        <Iyz>-0.00144774</Iyz>
+        <Izz>0.00298617</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.0496</y>
+          <z>-0.2258</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>482.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-20.0</x>
+            <y>-26.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>38.0</x>
+            <y>120.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>-26.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>94.0</y>
+            </control_point>
+            <control_point>
+              <x>-20.0</x>
+              <y>94.0</y>
+            </control_point>
+            <control_point>
+              <x>-20.0</x>
+              <y>-26.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.05</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_20_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0304</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_19_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RThighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0304</y>
+                <z>-0.5224</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_18_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RLowLeg</bodyname>
+      <parent>
+        <bodyname>RThigh</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>RKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>96.0</x>
+              <y>537.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>2.13974</mass>
+      <com>
+        <coordinates>
+          <x>0.00192208</x>
+          <y>-0.0038132</y>
+          <z>-0.125144</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.010726</Ixx>
+        <Ixy>2.21971E-5</Ixy>
+        <Ixz>-7.47347E-5</Ixz>
+        <Iyy>0.0107086</Iyy>
+        <Iyz>7.61197E-4</Iyz>
+        <Izz>0.00161252</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>99.0</x>
+              <y>637.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-19.0</x>
+            <y>-3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>44.0</x>
+            <y>113.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>25.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>25.0</x>
+              <y>110.0</y>
+            </control_point>
+            <control_point>
+              <x>-19.0</x>
+              <y>110.0</y>
+            </control_point>
+            <control_point>
+              <x>-19.0</x>
+              <y>-3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_17_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.301</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_16_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFoot</bodyname>
+      <parent>
+        <bodyname>RLowLeg</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>693.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>93.0</x>
+              <y>750.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-21.0</x>
+            <y>-12.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>55.0</x>
+            <y>85.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>34.0</x>
+              <y>-12.0</y>
+            </control_point>
+            <control_point>
+              <x>34.0</x>
+              <y>73.0</y>
+            </control_point>
+            <control_point>
+              <x>-21.0</x>
+              <y>73.0</y>
+            </control_point>
+            <control_point>
+              <x>-21.0</x>
+              <y>-12.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="153" G="153" B="153" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.036</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Right_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
+            </inline>
+            <box>
+              <name>Bar_right</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.08</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="153" G="153" B="153" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThigh</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_1</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>393.0</x>
+              <y>384.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>3.61731</mass>
+      <com>
+        <coordinates>
+          <x>7.39959E-4</x>
+          <y>0.0476521</y>
+          <z>-0.100464</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0271576</Ixx>
+        <Ixy>-2.80405E-5</Ixy>
+        <Ixz>1.32356E-4</Ixz>
+        <Iyy>0.0265146</Iyy>
+        <Iyz>0.0015218</Iyz>
+        <Izz>0.00298617</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0496</y>
+          <z>-0.2258</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>394.0</x>
+              <y>465.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-23.0</x>
+            <y>-9.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>53.0</x>
+            <y>109.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>30.0</x>
+              <y>-9.0</y>
+            </control_point>
+            <control_point>
+              <x>30.0</x>
+              <y>100.0</y>
+            </control_point>
+            <control_point>
+              <x>-23.0</x>
+              <y>100.0</y>
+            </control_point>
+            <control_point>
+              <x>-23.0</x>
+              <y>-9.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.04</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_06_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0304</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_05_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LThighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0284</y>
+                <z>-0.5224</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_04_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LLowLeg</bodyname>
+      <parent>
+        <bodyname>LThigh</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>LKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>404.0</x>
+              <y>543.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>2.13974</mass>
+      <com>
+        <coordinates>
+          <x>0.00192208</x>
+          <y>0.0038132</y>
+          <z>-0.125144</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.010726</Ixx>
+        <Ixy>-3.33531E-6</Ixy>
+        <Ixz>-7.47347E-5</Ixz>
+        <Iyy>0.0107086</Iyy>
+        <Iyz>-2.38516E-4</Iyz>
+        <Izz>0.00161252</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>407.0</x>
+              <y>628.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-26.0</x>
+            <y>-3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>52.0</x>
+            <y>98.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>26.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>26.0</x>
+              <y>95.0</y>
+            </control_point>
+            <control_point>
+              <x>-26.0</x>
+              <y>95.0</y>
+            </control_point>
+            <control_point>
+              <x>-26.0</x>
+              <y>-3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_03_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.301</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_02_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFoot</bodyname>
+      <parent>
+        <bodyname>LLowLeg</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>405.0</x>
+              <y>668.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>405.0</x>
+              <y>729.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-25.0</x>
+            <y>1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>50.0</x>
+            <y>74.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>25.0</x>
+              <y>1.0</y>
+            </control_point>
+            <control_point>
+              <x>25.0</x>
+              <y>75.0</y>
+            </control_point>
+            <control_point>
+              <x>-25.0</x>
+              <y>75.0</y>
+            </control_point>
+            <control_point>
+              <x>-25.0</x>
+              <y>1.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="153" G="153" B="153" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.037</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/cache/cache_foot.wrl</url>
+            </inline>
+            <inline>
+              <name>Left_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/feet/short/coman_short_foot.wrl</url>
+            </inline>
+            <box>
+              <name>Bar_left</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.08</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="153" G="153" B="153" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>Torso</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_2</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>251.0</x>
+              <y>299.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>6.90761</mass>
+      <com>
+        <coordinates>
+          <x>-0.032983</x>
+          <y>-7.54861E-4</y>
+          <z>0.154622</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0438747</Ixx>
+        <Ixy>1.78445E-4</Ixy>
+        <Ixz>0.00240963</Ixz>
+        <Iyy>0.0402919</Iyy>
+        <Iyz>-6.43229E-4</Iyz>
+        <Izz>0.0266284</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_0</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>-0.0825</y>
+          <z>0.2029</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>201.0</x>
+              <y>160.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_1</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>0.0825</y>
+          <z>0.2029</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>312.0</x>
+              <y>165.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-65.0</x>
+            <y>-152.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>140.0</x>
+            <y>165.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>75.0</x>
+              <y>-152.0</y>
+            </control_point>
+            <control_point>
+              <x>75.0</x>
+              <y>13.0</y>
+            </control_point>
+            <control_point>
+              <x>-65.0</x>
+              <y>13.0</y>
+            </control_point>
+            <control_point>
+              <x>-65.0</x>
+              <y>-152.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Torso_visu</name>
+              <position>
+                <x>-0.015</x>
+                <y>0.0</y>
+                <z>-0.6425</z>
+              </position>
+              <scale>
+                <x>0.0393</x>
+                <y>0.0393</y>
+                <z>0.0393</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/torso_blue.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RArm</bodyname>
+      <parent>
+        <bodyname>Torso</bodyname>
+        <pointname>Point_0</pointname>
+      </parent>
+      <joint>
+        <jointname>RShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>137.0</x>
+              <y>158.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>3.02396</mass>
+      <com>
+        <coordinates>
+          <x>0.00161196</x>
+          <y>-0.0675862</y>
+          <z>-0.0737184</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0199566</Ixx>
+        <Ixy>1.07891E-4</Ixy>
+        <Ixz>5.85641E-5</Ixz>
+        <Iyy>0.019132</Iyy>
+        <Iyz>-0.00161024</Iyz>
+        <Izz>0.00303321</Izz>
+      </inertia>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-41.0</x>
+            <y>-17.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>57.0</x>
+            <y>151.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>16.0</x>
+              <y>-17.0</y>
+            </control_point>
+            <control_point>
+              <x>16.0</x>
+              <y>134.0</y>
+            </control_point>
+            <control_point>
+              <x>-41.0</x>
+              <y>134.0</y>
+            </control_point>
+            <control_point>
+              <x>-41.0</x>
+              <y>-17.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_24_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.079</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_23_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.079</y>
+                <z>-0.8425</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_22_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>RElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.079</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_21_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LArm</bodyname>
+      <parent>
+        <bodyname>Torso</bodyname>
+        <pointname>Point_1</pointname>
+      </parent>
+      <joint>
+        <jointname>LShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>382.0</x>
+              <y>161.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>3.02396</mass>
+      <com>
+        <coordinates>
+          <x>0.00161196</x>
+          <y>0.0675862</y>
+          <z>-0.0737184</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0199566</Ixx>
+        <Ixy>1.48786E-4</Ixy>
+        <Ixz>5.85641E-5</Ixz>
+        <Iyy>0.019132</Iyy>
+        <Iyz>0.00214378</Iyz>
+        <Izz>0.00303321</Izz>
+      </inertia>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-24.0</x>
+            <y>-10.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>58.0</x>
+            <y>152.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>31.0</x>
+              <y>-10.0</y>
+            </control_point>
+            <control_point>
+              <x>31.0</x>
+              <y>136.0</y>
+            </control_point>
+            <control_point>
+              <x>-24.0</x>
+              <y>136.0</y>
+            </control_point>
+            <control_point>
+              <x>-24.0</x>
+              <y>-10.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_14_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.079</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_13_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.079</y>
+                <z>-0.8425</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_12_prt.wrl</url>
+            </inline>
+            <inline>
+              <name>LElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.079</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/animationR/vrml/IIT_files/asm0001_vrml_11_prt.wrl</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+  </bodytree>
+  <x3D>
+    <background>
+      <!--This is the background properties-->
+      <color R="255" G="255" B="255" />
+    </background>
+    <shapeScaleFactors>
+      <!--This is the scale factors-->
+      <frame>0.2</frame>
+      <joint>0.3</joint>
+      <com>0.3</com>
+      <point>0.3</point>
+    </shapeScaleFactors>
+    <lights>
+      <!--This is the light list-->
+      <directionalLight>
+        <name>DefaultLight01</name>
+        <joint>base</joint>
+        <active />
+        <direction>
+          <x>2.0</x>
+          <y>1.0</y>
+          <z>1.0</z>
+        </direction>
+      </directionalLight>
+      <directionalLight>
+        <name>DefaultLight02</name>
+        <joint>base</joint>
+        <active />
+        <direction>
+          <x>-1.0</x>
+          <y>-2.0</y>
+          <z>-1.0</z>
+        </direction>
+      </directionalLight>
+      <directionalLight>
+        <name>DefaultLight03</name>
+        <joint>base</joint>
+        <active />
+        <direction>
+          <x>1.0</x>
+          <y>-1.0</y>
+          <z>-2.0</z>
+        </direction>
+      </directionalLight>
+      <directionalLight>
+        <name>DefaultLight04</name>
+        <joint>base</joint>
+        <active />
+        <direction>
+          <x>-1.0</x>
+          <y>2.0</y>
+          <z>1.0</z>
+        </direction>
+      </directionalLight>
+    </lights>
+  </x3D>
+</mbsDescR7>
+
diff --git a/dataR/extra/flex_feet/specific_simu_index.cc b/dataR/extra/flex_feet/specific_simu_index.cc
new file mode 100644
index 0000000..deada52
--- /dev/null
+++ b/dataR/extra/flex_feet/specific_simu_index.cc
@@ -0,0 +1,259 @@
+#include "ModelSimuIndex.hh"
+#include "SimuIndex.hh"
+#include "user_all_id.h"
+
+#include <iostream>
+#include <stdlib.h>
+
+/*! \brief get the indexes related to the MBS file for the joints
+ * 
+ * \param[in] joint_id ID of the requested joint
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsJointId(int joint_id)
+{
+	switch (joint_id)
+	{
+		// floating base
+		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
+		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
+		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
+		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
+		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
+		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
+
+		// right leg
+		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
+		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
+		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
+		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
+		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
+		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
+		case SimuJointIndex::RightFlexProx  : return RFlex_pr_T3_id;
+		case SimuJointIndex::RightFlexDist  : return RFlex_ds_T3_id;
+
+		// left leg
+		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
+		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
+		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
+		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
+		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
+		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
+		case SimuJointIndex::LeftFlexProx  : return LFlex_pr_T3_id;
+		case SimuJointIndex::LeftFlexDist  : return LFlex_ds_T3_id;
+
+		// torso
+		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
+		case SimuJointIndex::TorsoPitch : return WaistSag_id;
+		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
+
+		// right arm
+		case SimuJointIndex::RightShPitch  : return RShSag_id;
+		case SimuJointIndex::RightShRoll   : return RShLat_id;
+		case SimuJointIndex::RightShYaw    : return RShYaw_id;
+		case SimuJointIndex::RightElbPitch : return RElb_id;
+
+		// left arm
+		case SimuJointIndex::LeftShPitch  : return LShSag_id;
+		case SimuJointIndex::LeftShRoll   : return LShLat_id;
+		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
+		case SimuJointIndex::LeftElbPitch : return LElb_id;
+	
+		default:
+			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the bodies
+ * 
+ * \param[in] body_id ID of the requested body
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsBodyId(int body_id)
+{
+	switch (body_id)
+	{
+		case SimuBodyIndex::Waist : return Waist_id;
+
+		case SimuBodyIndex::RHipmot : return RHipmot_id;
+		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
+		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
+		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
+		case SimuBodyIndex::RFootmot : return RFootmot_id;
+		case SimuBodyIndex::RFoot    : return RFoot_id;
+		case SimuBodyIndex::RFlexPr  : return RFlexPr_id;
+		case SimuBodyIndex::RFlexDs  : return RFlexDs_id;
+
+		case SimuBodyIndex::LHipmot : return LHipmot_id;
+		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
+		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
+		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
+		case SimuBodyIndex::LFootmot : return LFootmot_id;
+		case SimuBodyIndex::LFoot    : return LFoot_id;
+		case SimuBodyIndex::LFlexPr  : return LFlexPr_id;
+		case SimuBodyIndex::LFlexDs  : return LFlexDs_id;
+
+		case SimuBodyIndex::DWL : return DWL_id;
+		case SimuBodyIndex::DWS : return DWS_id;
+		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
+
+		case SimuBodyIndex::RShp : return RShp_id;
+		case SimuBodyIndex::RShr : return RShr_id;
+		case SimuBodyIndex::RShy : return RShy_id;
+		case SimuBodyIndex::RElb : return RElb_id;
+		case SimuBodyIndex::LShp : return LShp_id;
+		case SimuBodyIndex::LShr : return LShr_id;
+		case SimuBodyIndex::LShy : return LShy_id;
+		case SimuBodyIndex::LElb : return LElb_id;
+	
+		default:
+				std::cout << "Error: unknown body index: " << body_id << std::endl;
+				exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the S sensors
+ * 
+ * \param[in] Ssens_id ID of the requested S sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
+{
+	switch (Ssens_id)
+	{
+		// mid-waist
+		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
+
+		// upper feet part
+		case SimuSsensIndex::RightFoot : return RFoot_Upper_id;
+		case SimuSsensIndex::LeftFoot  : return LFoot_Upper_id;
+
+		// one part feet
+		case SimuSsensIndex::RightFlexProx : return mbs_data->Nsensor + RFlex_pr_force_id;
+		case SimuSsensIndex::RightFlexDist : return mbs_data->Nsensor + RFlex_ds_force_id;
+		case SimuSsensIndex::LeftFlexProx  : return mbs_data->Nsensor + LFlex_pr_force_id;
+		case SimuSsensIndex::LeftFlexDist  : return mbs_data->Nsensor + LFlex_ds_force_id;
+
+		// legs
+		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
+		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
+		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
+		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
+
+		// torso
+		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
+
+		// arms
+		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
+		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
+		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
+		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
+
+		// com
+		case SimuSsensIndex::WaistCg : return WaistCg_id;
+
+		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
+		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
+		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
+
+		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
+		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
+		case SimuSsensIndex::RFootCg    : return RFootCg_id;
+
+		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
+		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
+		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
+
+		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
+		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
+		case SimuSsensIndex::LFootCg    : return LFootCg_id;
+
+		case SimuSsensIndex::DWLCg : return DWLCg_id;
+		case SimuSsensIndex::DWSCg : return DWSCg_id;
+		case SimuSsensIndex::DWYCg : return DWYCg_id;
+
+		case SimuSsensIndex::RShpCg : return RShpCg_id;
+		case SimuSsensIndex::RShrCg : return RShrCg_id;
+		case SimuSsensIndex::RShyCg : return RShyCg_id;
+		case SimuSsensIndex::RElbCg : return RElbCg_id;
+
+		case SimuSsensIndex::LShpCg : return LShpCg_id;
+		case SimuSsensIndex::LShrCg : return LShrCg_id;
+		case SimuSsensIndex::LShyCg : return LShyCg_id;
+		case SimuSsensIndex::LElbCg : return LElbCg_id;
+	
+		default:
+			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the F sensors
+ * 
+ * \param[in] Fsens_id ID of the requested F sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
+{
+	switch (Fsens_id)
+	{
+		// mid-waist
+		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
+
+		// legs
+		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
+		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
+		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
+		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
+
+		// one part feet
+		case SimuFsensIndex::RightFlexProx : return RFlex_pr_force_id;
+		case SimuFsensIndex::RightFlexDist : return RFlex_ds_force_id;
+		case SimuFsensIndex::LeftFlexProx  : return LFlex_pr_force_id;
+		case SimuFsensIndex::LeftFlexDist  : return LFlex_ds_force_id;
+
+		// torso
+		case SimuFsensIndex::Torso : return Torso_contact_force_id;
+
+		// arms
+		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
+		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
+		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
+		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
+	
+		default:
+			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the links
+ * 
+ * \param[in] link_id ID of the requested link
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsLinkId(int link_id)
+{
+	switch (link_id)
+	{
+		case SimuLinkIndex::RightFlexProx : return Link_RFlexProx_id;
+		case SimuLinkIndex::RightFlexDist : return Link_RFlexDist_id;
+		case SimuLinkIndex::LeftFlexProx  : return Link_LFlexProx_id;
+		case SimuLinkIndex::LeftFlexDist  : return Link_LFlexDist_id;
+	
+		default:
+			std::cout << "Error: unknown link index: " << link_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
diff --git a/dataR/extra/flex_feet/user_kinematics_model.c b/dataR/extra/flex_feet/user_kinematics_model.c
new file mode 100644
index 0000000..c4a3b14
--- /dev/null
+++ b/dataR/extra/flex_feet/user_kinematics_model.c
@@ -0,0 +1,2083 @@
+#include "mbs_data.h"
+
+#include "math.h"
+
+/*! \brief compute the kinematics model of sensors with ExtForces
+ * 
+ * \param[in] mbs_data Robotran structure
+ * \param[out] PxF_tab tabular with all the 'PxF' computed
+ * \param[out] RxF_tab tabular with all the 'RxF' computed
+ * \param[out] VxF_tab tabular with all the 'VxF' computed
+ * \param[out] OMxF_tab tabular with all the 'OMxF' computed
+ */
+void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
+{
+    // variables declaration
+    int i, j;
+    double *q, *qd;
+    double **dpt;
+
+    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4], PxF13[4], PxF14[4];
+    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4], RxF13[4][4], RxF14[4][4];
+    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4], VxF13[4], VxF14[4];
+    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4], OMxF13[4], OMxF14[4];
+
+    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C15, C16, C17, C18, C19, C20, C23, C24, C25, C26, C27, C28, C29, C30, C31, C32, C33;
+    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S15, S16, S17, S18, S19, S20, S23, S24, S25, S26, S27, S28, S29, S30, S31, S32, S33;
+
+    double Dz133, Dz143, Dz213, Dz223;
+    double ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36;
+    double ROcp28_25, ROcp28_35, ROcp28_85, ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87;
+    double ROcp28_97, ROcp28_48, ROcp28_58, ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, OMcp28_25, OMcp28_35;
+    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
+    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
+    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
+    double VIcp28_19, VIcp28_29, VIcp28_39, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_17, ROcp29_27;
+    double ROcp29_37, ROcp29_77, ROcp29_87, ROcp29_97, ROcp29_48, ROcp29_58, ROcp29_68, ROcp29_78, ROcp29_88, ROcp29_98, ROcp29_19, ROcp29_29, ROcp29_39, ROcp29_49, ROcp29_59;
+    double ROcp29_69, ROcp29_110, ROcp29_210, ROcp29_310, ROcp29_710, ROcp29_810, ROcp29_910, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37;
+    double POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37, ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38;
+    double POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38, ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39;
+    double POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39, ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, RLcp29_110, RLcp29_210, RLcp29_310;
+    double POcp29_110, POcp29_210, POcp29_310, OMcp29_110, OMcp29_210, OMcp29_310, ORcp29_110, ORcp29_210, ORcp29_310, VIcp29_110, VIcp29_210, VIcp29_310, ROcp30_25, ROcp30_35, ROcp30_85;
+    double ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56, ROcp30_66, ROcp30_17, ROcp30_27, ROcp30_37, ROcp30_77, ROcp30_87, ROcp30_97, ROcp30_48, ROcp30_58;
+    double ROcp30_68, ROcp30_78, ROcp30_88, ROcp30_98, ROcp30_19, ROcp30_29, ROcp30_39, ROcp30_49, ROcp30_59, ROcp30_69, ROcp30_110, ROcp30_210, ROcp30_310, ROcp30_710, ROcp30_810;
+    double ROcp30_910, ROcp30_411, ROcp30_511, ROcp30_611, ROcp30_711, ROcp30_811, ROcp30_911, ROcp30_112, ROcp30_212, ROcp30_312, ROcp30_712, ROcp30_812, ROcp30_912, OMcp30_25, OMcp30_35;
+    double OMcp30_16, OMcp30_26, OMcp30_36, RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37;
+    double VIcp30_17, VIcp30_27, VIcp30_37, RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38;
+    double VIcp30_18, VIcp30_28, VIcp30_38, RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39;
+    double VIcp30_19, VIcp30_29, VIcp30_39, RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310;
+    double VIcp30_110, VIcp30_210, VIcp30_310, RLcp30_111, RLcp30_211, RLcp30_311, POcp30_111, POcp30_211, POcp30_311, OMcp30_111, OMcp30_211, OMcp30_311, ORcp30_111, ORcp30_211, ORcp30_311;
+    double VIcp30_111, VIcp30_211, VIcp30_311, OMcp30_112, OMcp30_212, OMcp30_312, RLcp30_113, RLcp30_213, RLcp30_313, POcp30_113, POcp30_213, POcp30_313, ORcp30_113, ORcp30_213, ORcp30_313;
+    double VIcp30_113, VIcp30_213, VIcp30_313, ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_17, ROcp31_27;
+    double ROcp31_37, ROcp31_77, ROcp31_87, ROcp31_97, ROcp31_48, ROcp31_58, ROcp31_68, ROcp31_78, ROcp31_88, ROcp31_98, ROcp31_19, ROcp31_29, ROcp31_39, ROcp31_49, ROcp31_59;
+    double ROcp31_69, ROcp31_110, ROcp31_210, ROcp31_310, ROcp31_710, ROcp31_810, ROcp31_910, ROcp31_411, ROcp31_511, ROcp31_611, ROcp31_711, ROcp31_811, ROcp31_911, ROcp31_112, ROcp31_212;
+    double ROcp31_312, ROcp31_712, ROcp31_812, ROcp31_912, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37;
+    double OMcp31_17, OMcp31_27, OMcp31_37, ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38;
+    double OMcp31_18, OMcp31_28, OMcp31_38, ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39;
+    double OMcp31_19, OMcp31_29, OMcp31_39, ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310;
+    double OMcp31_110, OMcp31_210, OMcp31_310, ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311;
+    double OMcp31_111, OMcp31_211, OMcp31_311, ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313;
+    double POcp31_113, POcp31_213, POcp31_313, ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26;
+    double ROcp32_36, ROcp32_46, ROcp32_56, ROcp32_66, ROcp32_115, ROcp32_215, ROcp32_315, ROcp32_715, ROcp32_815, ROcp32_915, ROcp32_416, ROcp32_516, ROcp32_616, ROcp32_716, ROcp32_816;
+    double ROcp32_916, ROcp32_117, ROcp32_217, ROcp32_317, ROcp32_417, ROcp32_517, ROcp32_617, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37;
+    double POcp32_17, POcp32_27, POcp32_37, OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, RLcp32_18, RLcp32_28, RLcp32_38;
+    double POcp32_18, POcp32_28, POcp32_38, OMcp32_18, OMcp32_28, OMcp32_38, ORcp32_18, ORcp32_28, ORcp32_38, VIcp32_18, VIcp32_28, VIcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
+    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
+    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_115, ROcp33_215, ROcp33_315, ROcp33_715, ROcp33_815, ROcp33_915, ROcp33_416, ROcp33_516;
+    double ROcp33_616, ROcp33_716, ROcp33_816, ROcp33_916, ROcp33_117, ROcp33_217, ROcp33_317, ROcp33_417, ROcp33_517, ROcp33_617, ROcp33_118, ROcp33_218, ROcp33_318, ROcp33_718, ROcp33_818;
+    double ROcp33_918, OMcp33_25, OMcp33_35, OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37;
+    double ORcp33_17, ORcp33_27, ORcp33_37, VIcp33_17, VIcp33_27, VIcp33_37, RLcp33_18, RLcp33_28, RLcp33_38, POcp33_18, POcp33_28, POcp33_38, OMcp33_18, OMcp33_28, OMcp33_38;
+    double ORcp33_18, ORcp33_28, ORcp33_38, VIcp33_18, VIcp33_28, VIcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
+    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
+    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
+    double ROcp34_66, ROcp34_115, ROcp34_215, ROcp34_315, ROcp34_715, ROcp34_815, ROcp34_915, ROcp34_416, ROcp34_516, ROcp34_616, ROcp34_716, ROcp34_816, ROcp34_916, ROcp34_117, ROcp34_217;
+    double ROcp34_317, ROcp34_417, ROcp34_517, ROcp34_617, ROcp34_118, ROcp34_218, ROcp34_318, ROcp34_718, ROcp34_818, ROcp34_918, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819;
+    double ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, OMcp34_25, OMcp34_35, OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37;
+    double POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37, VIcp34_17, VIcp34_27, VIcp34_37, RLcp34_18, RLcp34_28, RLcp34_38;
+    double POcp34_18, POcp34_28, POcp34_38, OMcp34_18, OMcp34_28, OMcp34_38, ORcp34_18, ORcp34_28, ORcp34_38, VIcp34_18, VIcp34_28, VIcp34_38, RLcp34_19, RLcp34_29, RLcp34_39;
+    double POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39, ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310;
+    double POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310, ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311;
+    double POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311, ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, OMcp34_112, OMcp34_212, OMcp34_312;
+    double RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85;
+    double ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56, ROcp35_66, ROcp35_115, ROcp35_215, ROcp35_315, ROcp35_715, ROcp35_815, ROcp35_915, ROcp35_416, ROcp35_516;
+    double ROcp35_616, ROcp35_716, ROcp35_816, ROcp35_916, ROcp35_117, ROcp35_217, ROcp35_317, ROcp35_417, ROcp35_517, ROcp35_617, ROcp35_118, ROcp35_218, ROcp35_318, ROcp35_718, ROcp35_818;
+    double ROcp35_918, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, OMcp35_25, OMcp35_35;
+    double OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37, POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37;
+    double VIcp35_17, VIcp35_27, VIcp35_37, RLcp35_18, RLcp35_28, RLcp35_38, POcp35_18, POcp35_28, POcp35_38, OMcp35_18, OMcp35_28, OMcp35_38, ORcp35_18, ORcp35_28, ORcp35_38;
+    double VIcp35_18, VIcp35_28, VIcp35_38, RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39;
+    double VIcp35_19, VIcp35_29, VIcp35_39, RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310;
+    double VIcp35_110, VIcp35_210, VIcp35_310, RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311;
+    double VIcp35_111, VIcp35_211, VIcp35_311, OMcp35_112, OMcp35_212, OMcp35_312, RLcp35_113, RLcp35_213, RLcp35_313, POcp35_113, POcp35_213, POcp35_313, ORcp35_113, ORcp35_213, ORcp35_313;
+    double VIcp35_113, VIcp35_213, VIcp35_313, ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_423, ROcp36_523;
+    double ROcp36_623, ROcp36_723, ROcp36_823, ROcp36_923, ROcp36_124, ROcp36_224, ROcp36_324, ROcp36_724, ROcp36_824, ROcp36_924, ROcp36_125, ROcp36_225, ROcp36_325, ROcp36_425, ROcp36_525;
+    double ROcp36_625, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37, POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37;
+    double ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38, RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39;
+    double OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39, ROcp37_25, ROcp37_35, ROcp37_85, ROcp37_95, ROcp37_16, ROcp37_26;
+    double ROcp37_36, ROcp37_46, ROcp37_56, ROcp37_66, ROcp37_423, ROcp37_523, ROcp37_623, ROcp37_723, ROcp37_823, ROcp37_923, ROcp37_124, ROcp37_224, ROcp37_324, ROcp37_724, ROcp37_824;
+    double ROcp37_924, ROcp37_125, ROcp37_225, ROcp37_325, ROcp37_425, ROcp37_525, ROcp37_625, ROcp37_126, ROcp37_226, ROcp37_326, ROcp37_726, ROcp37_826, ROcp37_926, ROcp37_427, ROcp37_527;
+    double ROcp37_627, ROcp37_727, ROcp37_827, ROcp37_927, ROcp37_128, ROcp37_228, ROcp37_328, ROcp37_428, ROcp37_528, ROcp37_628, OMcp37_25, OMcp37_35, OMcp37_16, OMcp37_26, OMcp37_36;
+    double RLcp37_17, RLcp37_27, RLcp37_37, POcp37_17, POcp37_27, POcp37_37, OMcp37_17, OMcp37_27, OMcp37_37, ORcp37_17, ORcp37_27, ORcp37_37, VIcp37_17, VIcp37_27, VIcp37_37;
+    double OMcp37_18, OMcp37_28, OMcp37_38, RLcp37_19, RLcp37_29, RLcp37_39, POcp37_19, POcp37_29, POcp37_39, OMcp37_19, OMcp37_29, OMcp37_39, ORcp37_19, ORcp37_29, ORcp37_39;
+    double VIcp37_19, VIcp37_29, VIcp37_39, RLcp37_110, RLcp37_210, RLcp37_310, POcp37_110, POcp37_210, POcp37_310, OMcp37_110, OMcp37_210, OMcp37_310, ORcp37_110, ORcp37_210, ORcp37_310;
+    double VIcp37_110, VIcp37_210, VIcp37_310, RLcp37_111, RLcp37_211, RLcp37_311, POcp37_111, POcp37_211, POcp37_311, OMcp37_111, OMcp37_211, OMcp37_311, ORcp37_111, ORcp37_211, ORcp37_311;
+    double VIcp37_111, VIcp37_211, VIcp37_311, RLcp37_112, RLcp37_212, RLcp37_312, POcp37_112, POcp37_212, POcp37_312, OMcp37_112, OMcp37_212, OMcp37_312, ORcp37_112, ORcp37_212, ORcp37_312;
+    double VIcp37_112, VIcp37_212, VIcp37_312, ROcp38_25, ROcp38_35, ROcp38_85, ROcp38_95, ROcp38_16, ROcp38_26, ROcp38_36, ROcp38_46, ROcp38_56, ROcp38_66, ROcp38_423, ROcp38_523;
+    double ROcp38_623, ROcp38_723, ROcp38_823, ROcp38_923, ROcp38_124, ROcp38_224, ROcp38_324, ROcp38_724, ROcp38_824, ROcp38_924, ROcp38_125, ROcp38_225, ROcp38_325, ROcp38_425, ROcp38_525;
+    double ROcp38_625, ROcp38_126, ROcp38_226, ROcp38_326, ROcp38_726, ROcp38_826, ROcp38_926, ROcp38_427, ROcp38_527, ROcp38_627, ROcp38_727, ROcp38_827, ROcp38_927, ROcp38_128, ROcp38_228;
+    double ROcp38_328, ROcp38_428, ROcp38_528, ROcp38_628, ROcp38_129, ROcp38_229, ROcp38_329, ROcp38_729, ROcp38_829, ROcp38_929, OMcp38_25, OMcp38_35, OMcp38_16, OMcp38_26, OMcp38_36;
+    double RLcp38_17, RLcp38_27, RLcp38_37, POcp38_17, POcp38_27, POcp38_37, OMcp38_17, OMcp38_27, OMcp38_37, ORcp38_17, ORcp38_27, ORcp38_37, VIcp38_17, VIcp38_27, VIcp38_37;
+    double OMcp38_18, OMcp38_28, OMcp38_38, RLcp38_19, RLcp38_29, RLcp38_39, POcp38_19, POcp38_29, POcp38_39, OMcp38_19, OMcp38_29, OMcp38_39, ORcp38_19, ORcp38_29, ORcp38_39;
+    double VIcp38_19, VIcp38_29, VIcp38_39, RLcp38_110, RLcp38_210, RLcp38_310, POcp38_110, POcp38_210, POcp38_310, OMcp38_110, OMcp38_210, OMcp38_310, ORcp38_110, ORcp38_210, ORcp38_310;
+    double VIcp38_110, VIcp38_210, VIcp38_310, RLcp38_111, RLcp38_211, RLcp38_311, POcp38_111, POcp38_211, POcp38_311, OMcp38_111, OMcp38_211, OMcp38_311, ORcp38_111, ORcp38_211, ORcp38_311;
+    double VIcp38_111, VIcp38_211, VIcp38_311, RLcp38_112, RLcp38_212, RLcp38_312, POcp38_112, POcp38_212, POcp38_312, OMcp38_112, OMcp38_212, OMcp38_312, ORcp38_112, ORcp38_212, ORcp38_312;
+    double VIcp38_112, VIcp38_212, VIcp38_312, RLcp38_113, RLcp38_213, RLcp38_313, POcp38_113, POcp38_213, POcp38_313, OMcp38_113, OMcp38_213, OMcp38_313, ORcp38_113, ORcp38_213, ORcp38_313;
+    double VIcp38_113, VIcp38_213, VIcp38_313, ROcp39_25, ROcp39_35, ROcp39_85, ROcp39_95, ROcp39_16, ROcp39_26, ROcp39_36, ROcp39_46, ROcp39_56, ROcp39_66, ROcp39_423, ROcp39_523;
+    double ROcp39_623, ROcp39_723, ROcp39_823, ROcp39_923, ROcp39_124, ROcp39_224, ROcp39_324, ROcp39_724, ROcp39_824, ROcp39_924, ROcp39_125, ROcp39_225, ROcp39_325, ROcp39_425, ROcp39_525;
+    double ROcp39_625, ROcp39_130, ROcp39_230, ROcp39_330, ROcp39_730, ROcp39_830, ROcp39_930, ROcp39_431, ROcp39_531, ROcp39_631, ROcp39_731, ROcp39_831, ROcp39_931, ROcp39_132, ROcp39_232;
+    double ROcp39_332, ROcp39_432, ROcp39_532, ROcp39_632, OMcp39_25, OMcp39_35, OMcp39_16, OMcp39_26, OMcp39_36, RLcp39_17, RLcp39_27, RLcp39_37, POcp39_17, POcp39_27, POcp39_37;
+    double OMcp39_17, OMcp39_27, OMcp39_37, ORcp39_17, ORcp39_27, ORcp39_37, VIcp39_17, VIcp39_27, VIcp39_37, OMcp39_18, OMcp39_28, OMcp39_38, RLcp39_19, RLcp39_29, RLcp39_39;
+    double POcp39_19, POcp39_29, POcp39_39, OMcp39_19, OMcp39_29, OMcp39_39, ORcp39_19, ORcp39_29, ORcp39_39, VIcp39_19, VIcp39_29, VIcp39_39, RLcp39_110, RLcp39_210, RLcp39_310;
+    double POcp39_110, POcp39_210, POcp39_310, OMcp39_110, OMcp39_210, OMcp39_310, ORcp39_110, ORcp39_210, ORcp39_310, VIcp39_110, VIcp39_210, VIcp39_310, RLcp39_111, RLcp39_211, RLcp39_311;
+    double POcp39_111, POcp39_211, POcp39_311, OMcp39_111, OMcp39_211, OMcp39_311, ORcp39_111, ORcp39_211, ORcp39_311, VIcp39_111, VIcp39_211, VIcp39_311, RLcp39_112, RLcp39_212, RLcp39_312;
+    double POcp39_112, POcp39_212, POcp39_312, OMcp39_112, OMcp39_212, OMcp39_312, ORcp39_112, ORcp39_212, ORcp39_312, VIcp39_112, VIcp39_212, VIcp39_312, ROcp40_25, ROcp40_35, ROcp40_85;
+    double ROcp40_95, ROcp40_16, ROcp40_26, ROcp40_36, ROcp40_46, ROcp40_56, ROcp40_66, ROcp40_423, ROcp40_523, ROcp40_623, ROcp40_723, ROcp40_823, ROcp40_923, ROcp40_124, ROcp40_224;
+    double ROcp40_324, ROcp40_724, ROcp40_824, ROcp40_924, ROcp40_125, ROcp40_225, ROcp40_325, ROcp40_425, ROcp40_525, ROcp40_625, ROcp40_130, ROcp40_230, ROcp40_330, ROcp40_730, ROcp40_830;
+    double ROcp40_930, ROcp40_431, ROcp40_531, ROcp40_631, ROcp40_731, ROcp40_831, ROcp40_931, ROcp40_132, ROcp40_232, ROcp40_332, ROcp40_432, ROcp40_532, ROcp40_632, ROcp40_133, ROcp40_233;
+    double ROcp40_333, ROcp40_733, ROcp40_833, ROcp40_933, OMcp40_25, OMcp40_35, OMcp40_16, OMcp40_26, OMcp40_36, RLcp40_17, RLcp40_27, RLcp40_37, POcp40_17, POcp40_27, POcp40_37;
+    double OMcp40_17, OMcp40_27, OMcp40_37, ORcp40_17, ORcp40_27, ORcp40_37, VIcp40_17, VIcp40_27, VIcp40_37, OMcp40_18, OMcp40_28, OMcp40_38, RLcp40_19, RLcp40_29, RLcp40_39;
+    double POcp40_19, POcp40_29, POcp40_39, OMcp40_19, OMcp40_29, OMcp40_39, ORcp40_19, ORcp40_29, ORcp40_39, VIcp40_19, VIcp40_29, VIcp40_39, RLcp40_110, RLcp40_210, RLcp40_310;
+    double POcp40_110, POcp40_210, POcp40_310, OMcp40_110, OMcp40_210, OMcp40_310, ORcp40_110, ORcp40_210, ORcp40_310, VIcp40_110, VIcp40_210, VIcp40_310, RLcp40_111, RLcp40_211, RLcp40_311;
+    double POcp40_111, POcp40_211, POcp40_311, OMcp40_111, OMcp40_211, OMcp40_311, ORcp40_111, ORcp40_211, ORcp40_311, VIcp40_111, VIcp40_211, VIcp40_311, RLcp40_112, RLcp40_212, RLcp40_312;
+    double POcp40_112, POcp40_212, POcp40_312, OMcp40_112, OMcp40_212, OMcp40_312, ORcp40_112, ORcp40_212, ORcp40_312, VIcp40_112, VIcp40_212, VIcp40_312, RLcp40_113, RLcp40_213, RLcp40_313;
+    double POcp40_113, POcp40_213, POcp40_313, OMcp40_113, OMcp40_213, OMcp40_313, ORcp40_113, ORcp40_213, ORcp40_313, VIcp40_113, VIcp40_213, VIcp40_313;
+
+    // variables initialization
+    q   = mbs_data->q;
+    qd  = mbs_data->qd;
+    dpt = mbs_data->dpt;
+
+    // cosines and sines
+    C4 = cos(q[4]);
+    C5 = cos(q[5]);
+    C6 = cos(q[6]);
+    C7 = cos(q[7]);
+    C8 = cos(q[8]);
+    C9 = cos(q[9]);
+    C10 = cos(q[10]);
+    C11 = cos(q[11]);
+    C12 = cos(q[12]);
+    C15 = cos(q[15]);
+    C16 = cos(q[16]);
+    C17 = cos(q[17]);
+    C18 = cos(q[18]);
+    C19 = cos(q[19]);
+    C20 = cos(q[20]);
+    C23 = cos(q[23]);
+    C24 = cos(q[24]);
+    C25 = cos(q[25]);
+    C26 = cos(q[26]);
+    C27 = cos(q[27]);
+    C28 = cos(q[28]);
+    C29 = cos(q[29]);
+    C30 = cos(q[30]);
+    C31 = cos(q[31]);
+    C32 = cos(q[32]);
+    C33 = cos(q[33]);
+    S4 = sin(q[4]);
+    S5 = sin(q[5]);
+    S6 = sin(q[6]);
+    S7 = sin(q[7]);
+    S8 = sin(q[8]);
+    S9 = sin(q[9]);
+    S10 = sin(q[10]);
+    S11 = sin(q[11]);
+    S12 = sin(q[12]);
+    S15 = sin(q[15]);
+    S16 = sin(q[16]);
+    S17 = sin(q[17]);
+    S18 = sin(q[18]);
+    S19 = sin(q[19]);
+    S20 = sin(q[20]);
+    S23 = sin(q[23]);
+    S24 = sin(q[24]);
+    S25 = sin(q[25]);
+    S26 = sin(q[26]);
+    S27 = sin(q[27]);
+    S28 = sin(q[28]);
+    S29 = sin(q[29]);
+    S30 = sin(q[30]);
+    S31 = sin(q[31]);
+    S32 = sin(q[32]);
+    S33 = sin(q[33]);
+
+    // main computation
+    Dz133 = q[13]+dpt[3][20];
+    Dz143 = q[14]+dpt[3][21];
+    Dz213 = q[21]+dpt[3][38];
+    Dz223 = q[22]+dpt[3][39];
+    ROcp27_25 = S4*S5;
+    ROcp27_35 = -C4*S5;
+    ROcp27_85 = -S4*C5;
+    ROcp27_95 = C4*C5;
+    ROcp27_16 = C5*C6;
+    ROcp27_26 = ROcp27_25*C6+C4*S6;
+    ROcp27_36 = ROcp27_35*C6+S4*S6;
+    ROcp27_46 = -C5*S6;
+    ROcp27_56 = -ROcp27_25*S6+C4*C6;
+    ROcp27_66 = -ROcp27_35*S6+S4*C6;
+    OMcp27_25 = qd[5]*C4;
+    OMcp27_35 = qd[5]*S4;
+    OMcp27_16 = qd[4]+qd[6]*S5;
+    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
+    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
+    PxF1[1] = q[1];
+    PxF1[2] = q[2];
+    PxF1[3] = q[3];
+    RxF1[1][1] = ROcp27_16;
+    RxF1[1][2] = ROcp27_26;
+    RxF1[1][3] = ROcp27_36;
+    RxF1[2][1] = ROcp27_46;
+    RxF1[2][2] = ROcp27_56;
+    RxF1[2][3] = ROcp27_66;
+    RxF1[3][1] = S5;
+    RxF1[3][2] = ROcp27_85;
+    RxF1[3][3] = ROcp27_95;
+    VxF1[1] = qd[1];
+    VxF1[2] = qd[2];
+    VxF1[3] = qd[3];
+    OMxF1[1] = OMcp27_16;
+    OMxF1[2] = OMcp27_26;
+    OMxF1[3] = OMcp27_36;
+    ROcp28_25 = S4*S5;
+    ROcp28_35 = -C4*S5;
+    ROcp28_85 = -S4*C5;
+    ROcp28_95 = C4*C5;
+    ROcp28_16 = C5*C6;
+    ROcp28_26 = ROcp28_25*C6+C4*S6;
+    ROcp28_36 = ROcp28_35*C6+S4*S6;
+    ROcp28_46 = -C5*S6;
+    ROcp28_56 = -ROcp28_25*S6+C4*C6;
+    ROcp28_66 = -ROcp28_35*S6+S4*C6;
+    ROcp28_17 = ROcp28_16*C7-S5*S7;
+    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
+    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
+    ROcp28_77 = ROcp28_16*S7+S5*C7;
+    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
+    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
+    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
+    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
+    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
+    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
+    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
+    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
+    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
+    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
+    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
+    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
+    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
+    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
+    OMcp28_25 = qd[5]*C4;
+    OMcp28_35 = qd[5]*S4;
+    OMcp28_16 = qd[4]+qd[6]*S5;
+    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
+    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
+    RLcp28_17 = ROcp28_46*dpt[2][2];
+    RLcp28_27 = ROcp28_56*dpt[2][2];
+    RLcp28_37 = ROcp28_66*dpt[2][2];
+    POcp28_17 = q[1]+RLcp28_17;
+    POcp28_27 = q[2]+RLcp28_27;
+    POcp28_37 = q[3]+RLcp28_37;
+    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
+    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
+    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
+    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
+    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
+    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
+    VIcp28_17 = qd[1]+ORcp28_17;
+    VIcp28_27 = qd[2]+ORcp28_27;
+    VIcp28_37 = qd[3]+ORcp28_37;
+    RLcp28_18 = ROcp28_46*dpt[2][6];
+    RLcp28_28 = ROcp28_56*dpt[2][6];
+    RLcp28_38 = ROcp28_66*dpt[2][6];
+    POcp28_18 = POcp28_17+RLcp28_18;
+    POcp28_28 = POcp28_27+RLcp28_28;
+    POcp28_38 = POcp28_37+RLcp28_38;
+    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
+    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
+    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
+    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
+    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
+    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
+    VIcp28_18 = ORcp28_18+VIcp28_17;
+    VIcp28_28 = ORcp28_28+VIcp28_27;
+    VIcp28_38 = ORcp28_38+VIcp28_37;
+    RLcp28_19 = ROcp28_78*dpt[3][8];
+    RLcp28_29 = ROcp28_88*dpt[3][8];
+    RLcp28_39 = ROcp28_98*dpt[3][8];
+    POcp28_19 = POcp28_18+RLcp28_19;
+    POcp28_29 = POcp28_28+RLcp28_29;
+    POcp28_39 = POcp28_38+RLcp28_39;
+    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
+    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
+    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
+    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
+    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
+    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
+    VIcp28_19 = ORcp28_19+VIcp28_18;
+    VIcp28_29 = ORcp28_29+VIcp28_28;
+    VIcp28_39 = ORcp28_39+VIcp28_38;
+    PxF2[1] = POcp28_19;
+    PxF2[2] = POcp28_29;
+    PxF2[3] = POcp28_39;
+    RxF2[1][1] = ROcp28_19;
+    RxF2[1][2] = ROcp28_29;
+    RxF2[1][3] = ROcp28_39;
+    RxF2[2][1] = ROcp28_49;
+    RxF2[2][2] = ROcp28_59;
+    RxF2[2][3] = ROcp28_69;
+    RxF2[3][1] = ROcp28_78;
+    RxF2[3][2] = ROcp28_88;
+    RxF2[3][3] = ROcp28_98;
+    VxF2[1] = VIcp28_19;
+    VxF2[2] = VIcp28_29;
+    VxF2[3] = VIcp28_39;
+    OMxF2[1] = OMcp28_19;
+    OMxF2[2] = OMcp28_29;
+    OMxF2[3] = OMcp28_39;
+    ROcp29_25 = S4*S5;
+    ROcp29_35 = -C4*S5;
+    ROcp29_85 = -S4*C5;
+    ROcp29_95 = C4*C5;
+    ROcp29_16 = C5*C6;
+    ROcp29_26 = ROcp29_25*C6+C4*S6;
+    ROcp29_36 = ROcp29_35*C6+S4*S6;
+    ROcp29_46 = -C5*S6;
+    ROcp29_56 = -ROcp29_25*S6+C4*C6;
+    ROcp29_66 = -ROcp29_35*S6+S4*C6;
+    ROcp29_17 = ROcp29_16*C7-S5*S7;
+    ROcp29_27 = ROcp29_26*C7-ROcp29_85*S7;
+    ROcp29_37 = ROcp29_36*C7-ROcp29_95*S7;
+    ROcp29_77 = ROcp29_16*S7+S5*C7;
+    ROcp29_87 = ROcp29_26*S7+ROcp29_85*C7;
+    ROcp29_97 = ROcp29_36*S7+ROcp29_95*C7;
+    ROcp29_48 = ROcp29_46*C8+ROcp29_77*S8;
+    ROcp29_58 = ROcp29_56*C8+ROcp29_87*S8;
+    ROcp29_68 = ROcp29_66*C8+ROcp29_97*S8;
+    ROcp29_78 = -ROcp29_46*S8+ROcp29_77*C8;
+    ROcp29_88 = -ROcp29_56*S8+ROcp29_87*C8;
+    ROcp29_98 = -ROcp29_66*S8+ROcp29_97*C8;
+    ROcp29_19 = ROcp29_17*C9+ROcp29_48*S9;
+    ROcp29_29 = ROcp29_27*C9+ROcp29_58*S9;
+    ROcp29_39 = ROcp29_37*C9+ROcp29_68*S9;
+    ROcp29_49 = -ROcp29_17*S9+ROcp29_48*C9;
+    ROcp29_59 = -ROcp29_27*S9+ROcp29_58*C9;
+    ROcp29_69 = -ROcp29_37*S9+ROcp29_68*C9;
+    ROcp29_110 = ROcp29_19*C10-ROcp29_78*S10;
+    ROcp29_210 = ROcp29_29*C10-ROcp29_88*S10;
+    ROcp29_310 = ROcp29_39*C10-ROcp29_98*S10;
+    ROcp29_710 = ROcp29_19*S10+ROcp29_78*C10;
+    ROcp29_810 = ROcp29_29*S10+ROcp29_88*C10;
+    ROcp29_910 = ROcp29_39*S10+ROcp29_98*C10;
+    OMcp29_25 = qd[5]*C4;
+    OMcp29_35 = qd[5]*S4;
+    OMcp29_16 = qd[4]+qd[6]*S5;
+    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
+    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
+    RLcp29_17 = ROcp29_46*dpt[2][2];
+    RLcp29_27 = ROcp29_56*dpt[2][2];
+    RLcp29_37 = ROcp29_66*dpt[2][2];
+    POcp29_17 = q[1]+RLcp29_17;
+    POcp29_27 = q[2]+RLcp29_27;
+    POcp29_37 = q[3]+RLcp29_37;
+    OMcp29_17 = OMcp29_16+qd[7]*ROcp29_46;
+    OMcp29_27 = OMcp29_26+qd[7]*ROcp29_56;
+    OMcp29_37 = OMcp29_36+qd[7]*ROcp29_66;
+    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
+    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
+    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
+    VIcp29_17 = qd[1]+ORcp29_17;
+    VIcp29_27 = qd[2]+ORcp29_27;
+    VIcp29_37 = qd[3]+ORcp29_37;
+    RLcp29_18 = ROcp29_46*dpt[2][6];
+    RLcp29_28 = ROcp29_56*dpt[2][6];
+    RLcp29_38 = ROcp29_66*dpt[2][6];
+    POcp29_18 = POcp29_17+RLcp29_18;
+    POcp29_28 = POcp29_27+RLcp29_28;
+    POcp29_38 = POcp29_37+RLcp29_38;
+    OMcp29_18 = OMcp29_17+qd[8]*ROcp29_17;
+    OMcp29_28 = OMcp29_27+qd[8]*ROcp29_27;
+    OMcp29_38 = OMcp29_37+qd[8]*ROcp29_37;
+    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
+    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
+    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
+    VIcp29_18 = ORcp29_18+VIcp29_17;
+    VIcp29_28 = ORcp29_28+VIcp29_27;
+    VIcp29_38 = ORcp29_38+VIcp29_37;
+    RLcp29_19 = ROcp29_78*dpt[3][8];
+    RLcp29_29 = ROcp29_88*dpt[3][8];
+    RLcp29_39 = ROcp29_98*dpt[3][8];
+    POcp29_19 = POcp29_18+RLcp29_19;
+    POcp29_29 = POcp29_28+RLcp29_29;
+    POcp29_39 = POcp29_38+RLcp29_39;
+    OMcp29_19 = OMcp29_18+qd[9]*ROcp29_78;
+    OMcp29_29 = OMcp29_28+qd[9]*ROcp29_88;
+    OMcp29_39 = OMcp29_38+qd[9]*ROcp29_98;
+    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
+    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
+    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
+    VIcp29_19 = ORcp29_19+VIcp29_18;
+    VIcp29_29 = ORcp29_29+VIcp29_28;
+    VIcp29_39 = ORcp29_39+VIcp29_38;
+    RLcp29_110 = ROcp29_78*dpt[3][11];
+    RLcp29_210 = ROcp29_88*dpt[3][11];
+    RLcp29_310 = ROcp29_98*dpt[3][11];
+    POcp29_110 = POcp29_19+RLcp29_110;
+    POcp29_210 = POcp29_29+RLcp29_210;
+    POcp29_310 = POcp29_39+RLcp29_310;
+    OMcp29_110 = OMcp29_19+qd[10]*ROcp29_49;
+    OMcp29_210 = OMcp29_29+qd[10]*ROcp29_59;
+    OMcp29_310 = OMcp29_39+qd[10]*ROcp29_69;
+    ORcp29_110 = OMcp29_29*RLcp29_310-OMcp29_39*RLcp29_210;
+    ORcp29_210 = -OMcp29_19*RLcp29_310+OMcp29_39*RLcp29_110;
+    ORcp29_310 = OMcp29_19*RLcp29_210-OMcp29_29*RLcp29_110;
+    VIcp29_110 = ORcp29_110+VIcp29_19;
+    VIcp29_210 = ORcp29_210+VIcp29_29;
+    VIcp29_310 = ORcp29_310+VIcp29_39;
+    PxF3[1] = POcp29_110;
+    PxF3[2] = POcp29_210;
+    PxF3[3] = POcp29_310;
+    RxF3[1][1] = ROcp29_110;
+    RxF3[1][2] = ROcp29_210;
+    RxF3[1][3] = ROcp29_310;
+    RxF3[2][1] = ROcp29_49;
+    RxF3[2][2] = ROcp29_59;
+    RxF3[2][3] = ROcp29_69;
+    RxF3[3][1] = ROcp29_710;
+    RxF3[3][2] = ROcp29_810;
+    RxF3[3][3] = ROcp29_910;
+    VxF3[1] = VIcp29_110;
+    VxF3[2] = VIcp29_210;
+    VxF3[3] = VIcp29_310;
+    OMxF3[1] = OMcp29_110;
+    OMxF3[2] = OMcp29_210;
+    OMxF3[3] = OMcp29_310;
+    ROcp30_25 = S4*S5;
+    ROcp30_35 = -C4*S5;
+    ROcp30_85 = -S4*C5;
+    ROcp30_95 = C4*C5;
+    ROcp30_16 = C5*C6;
+    ROcp30_26 = ROcp30_25*C6+C4*S6;
+    ROcp30_36 = ROcp30_35*C6+S4*S6;
+    ROcp30_46 = -C5*S6;
+    ROcp30_56 = -ROcp30_25*S6+C4*C6;
+    ROcp30_66 = -ROcp30_35*S6+S4*C6;
+    ROcp30_17 = ROcp30_16*C7-S5*S7;
+    ROcp30_27 = ROcp30_26*C7-ROcp30_85*S7;
+    ROcp30_37 = ROcp30_36*C7-ROcp30_95*S7;
+    ROcp30_77 = ROcp30_16*S7+S5*C7;
+    ROcp30_87 = ROcp30_26*S7+ROcp30_85*C7;
+    ROcp30_97 = ROcp30_36*S7+ROcp30_95*C7;
+    ROcp30_48 = ROcp30_46*C8+ROcp30_77*S8;
+    ROcp30_58 = ROcp30_56*C8+ROcp30_87*S8;
+    ROcp30_68 = ROcp30_66*C8+ROcp30_97*S8;
+    ROcp30_78 = -ROcp30_46*S8+ROcp30_77*C8;
+    ROcp30_88 = -ROcp30_56*S8+ROcp30_87*C8;
+    ROcp30_98 = -ROcp30_66*S8+ROcp30_97*C8;
+    ROcp30_19 = ROcp30_17*C9+ROcp30_48*S9;
+    ROcp30_29 = ROcp30_27*C9+ROcp30_58*S9;
+    ROcp30_39 = ROcp30_37*C9+ROcp30_68*S9;
+    ROcp30_49 = -ROcp30_17*S9+ROcp30_48*C9;
+    ROcp30_59 = -ROcp30_27*S9+ROcp30_58*C9;
+    ROcp30_69 = -ROcp30_37*S9+ROcp30_68*C9;
+    ROcp30_110 = ROcp30_19*C10-ROcp30_78*S10;
+    ROcp30_210 = ROcp30_29*C10-ROcp30_88*S10;
+    ROcp30_310 = ROcp30_39*C10-ROcp30_98*S10;
+    ROcp30_710 = ROcp30_19*S10+ROcp30_78*C10;
+    ROcp30_810 = ROcp30_29*S10+ROcp30_88*C10;
+    ROcp30_910 = ROcp30_39*S10+ROcp30_98*C10;
+    ROcp30_411 = ROcp30_49*C11+ROcp30_710*S11;
+    ROcp30_511 = ROcp30_59*C11+ROcp30_810*S11;
+    ROcp30_611 = ROcp30_69*C11+ROcp30_910*S11;
+    ROcp30_711 = -ROcp30_49*S11+ROcp30_710*C11;
+    ROcp30_811 = -ROcp30_59*S11+ROcp30_810*C11;
+    ROcp30_911 = -ROcp30_69*S11+ROcp30_910*C11;
+    ROcp30_112 = ROcp30_110*C12-ROcp30_711*S12;
+    ROcp30_212 = ROcp30_210*C12-ROcp30_811*S12;
+    ROcp30_312 = ROcp30_310*C12-ROcp30_911*S12;
+    ROcp30_712 = ROcp30_110*S12+ROcp30_711*C12;
+    ROcp30_812 = ROcp30_210*S12+ROcp30_811*C12;
+    ROcp30_912 = ROcp30_310*S12+ROcp30_911*C12;
+    OMcp30_25 = qd[5]*C4;
+    OMcp30_35 = qd[5]*S4;
+    OMcp30_16 = qd[4]+qd[6]*S5;
+    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
+    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
+    RLcp30_17 = ROcp30_46*dpt[2][2];
+    RLcp30_27 = ROcp30_56*dpt[2][2];
+    RLcp30_37 = ROcp30_66*dpt[2][2];
+    POcp30_17 = q[1]+RLcp30_17;
+    POcp30_27 = q[2]+RLcp30_27;
+    POcp30_37 = q[3]+RLcp30_37;
+    OMcp30_17 = OMcp30_16+qd[7]*ROcp30_46;
+    OMcp30_27 = OMcp30_26+qd[7]*ROcp30_56;
+    OMcp30_37 = OMcp30_36+qd[7]*ROcp30_66;
+    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
+    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
+    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
+    VIcp30_17 = qd[1]+ORcp30_17;
+    VIcp30_27 = qd[2]+ORcp30_27;
+    VIcp30_37 = qd[3]+ORcp30_37;
+    RLcp30_18 = ROcp30_46*dpt[2][6];
+    RLcp30_28 = ROcp30_56*dpt[2][6];
+    RLcp30_38 = ROcp30_66*dpt[2][6];
+    POcp30_18 = POcp30_17+RLcp30_18;
+    POcp30_28 = POcp30_27+RLcp30_28;
+    POcp30_38 = POcp30_37+RLcp30_38;
+    OMcp30_18 = OMcp30_17+qd[8]*ROcp30_17;
+    OMcp30_28 = OMcp30_27+qd[8]*ROcp30_27;
+    OMcp30_38 = OMcp30_37+qd[8]*ROcp30_37;
+    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
+    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
+    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
+    VIcp30_18 = ORcp30_18+VIcp30_17;
+    VIcp30_28 = ORcp30_28+VIcp30_27;
+    VIcp30_38 = ORcp30_38+VIcp30_37;
+    RLcp30_19 = ROcp30_78*dpt[3][8];
+    RLcp30_29 = ROcp30_88*dpt[3][8];
+    RLcp30_39 = ROcp30_98*dpt[3][8];
+    POcp30_19 = POcp30_18+RLcp30_19;
+    POcp30_29 = POcp30_28+RLcp30_29;
+    POcp30_39 = POcp30_38+RLcp30_39;
+    OMcp30_19 = OMcp30_18+qd[9]*ROcp30_78;
+    OMcp30_29 = OMcp30_28+qd[9]*ROcp30_88;
+    OMcp30_39 = OMcp30_38+qd[9]*ROcp30_98;
+    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
+    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
+    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
+    VIcp30_19 = ORcp30_19+VIcp30_18;
+    VIcp30_29 = ORcp30_29+VIcp30_28;
+    VIcp30_39 = ORcp30_39+VIcp30_38;
+    RLcp30_110 = ROcp30_78*dpt[3][11];
+    RLcp30_210 = ROcp30_88*dpt[3][11];
+    RLcp30_310 = ROcp30_98*dpt[3][11];
+    POcp30_110 = POcp30_19+RLcp30_110;
+    POcp30_210 = POcp30_29+RLcp30_210;
+    POcp30_310 = POcp30_39+RLcp30_310;
+    OMcp30_110 = OMcp30_19+qd[10]*ROcp30_49;
+    OMcp30_210 = OMcp30_29+qd[10]*ROcp30_59;
+    OMcp30_310 = OMcp30_39+qd[10]*ROcp30_69;
+    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
+    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
+    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
+    VIcp30_110 = ORcp30_110+VIcp30_19;
+    VIcp30_210 = ORcp30_210+VIcp30_29;
+    VIcp30_310 = ORcp30_310+VIcp30_39;
+    RLcp30_111 = ROcp30_710*dpt[3][14];
+    RLcp30_211 = ROcp30_810*dpt[3][14];
+    RLcp30_311 = ROcp30_910*dpt[3][14];
+    POcp30_111 = POcp30_110+RLcp30_111;
+    POcp30_211 = POcp30_210+RLcp30_211;
+    POcp30_311 = POcp30_310+RLcp30_311;
+    OMcp30_111 = OMcp30_110+qd[11]*ROcp30_110;
+    OMcp30_211 = OMcp30_210+qd[11]*ROcp30_210;
+    OMcp30_311 = OMcp30_310+qd[11]*ROcp30_310;
+    ORcp30_111 = OMcp30_210*RLcp30_311-OMcp30_310*RLcp30_211;
+    ORcp30_211 = -OMcp30_110*RLcp30_311+OMcp30_310*RLcp30_111;
+    ORcp30_311 = OMcp30_110*RLcp30_211-OMcp30_210*RLcp30_111;
+    VIcp30_111 = ORcp30_111+VIcp30_110;
+    VIcp30_211 = ORcp30_211+VIcp30_210;
+    VIcp30_311 = ORcp30_311+VIcp30_310;
+    OMcp30_112 = OMcp30_111+qd[12]*ROcp30_411;
+    OMcp30_212 = OMcp30_211+qd[12]*ROcp30_511;
+    OMcp30_312 = OMcp30_311+qd[12]*ROcp30_611;
+    RLcp30_113 = ROcp30_112*dpt[1][20]+ROcp30_712*Dz133;
+    RLcp30_213 = ROcp30_212*dpt[1][20]+ROcp30_812*Dz133;
+    RLcp30_313 = ROcp30_312*dpt[1][20]+ROcp30_912*Dz133;
+    POcp30_113 = POcp30_111+RLcp30_113;
+    POcp30_213 = POcp30_211+RLcp30_213;
+    POcp30_313 = POcp30_311+RLcp30_313;
+    ORcp30_113 = OMcp30_212*RLcp30_313-OMcp30_312*RLcp30_213;
+    ORcp30_213 = -OMcp30_112*RLcp30_313+OMcp30_312*RLcp30_113;
+    ORcp30_313 = OMcp30_112*RLcp30_213-OMcp30_212*RLcp30_113;
+    VIcp30_113 = ORcp30_113+VIcp30_111+qd[13]*ROcp30_712;
+    VIcp30_213 = ORcp30_213+VIcp30_211+qd[13]*ROcp30_812;
+    VIcp30_313 = ORcp30_313+VIcp30_311+qd[13]*ROcp30_912;
+    PxF4[1] = POcp30_113;
+    PxF4[2] = POcp30_213;
+    PxF4[3] = POcp30_313;
+    RxF4[1][1] = ROcp30_112;
+    RxF4[1][2] = ROcp30_212;
+    RxF4[1][3] = ROcp30_312;
+    RxF4[2][1] = ROcp30_411;
+    RxF4[2][2] = ROcp30_511;
+    RxF4[2][3] = ROcp30_611;
+    RxF4[3][1] = ROcp30_712;
+    RxF4[3][2] = ROcp30_812;
+    RxF4[3][3] = ROcp30_912;
+    VxF4[1] = VIcp30_113;
+    VxF4[2] = VIcp30_213;
+    VxF4[3] = VIcp30_313;
+    OMxF4[1] = OMcp30_112;
+    OMxF4[2] = OMcp30_212;
+    OMxF4[3] = OMcp30_312;
+    ROcp31_25 = S4*S5;
+    ROcp31_35 = -C4*S5;
+    ROcp31_85 = -S4*C5;
+    ROcp31_95 = C4*C5;
+    ROcp31_16 = C5*C6;
+    ROcp31_26 = ROcp31_25*C6+C4*S6;
+    ROcp31_36 = ROcp31_35*C6+S4*S6;
+    ROcp31_46 = -C5*S6;
+    ROcp31_56 = -ROcp31_25*S6+C4*C6;
+    ROcp31_66 = -ROcp31_35*S6+S4*C6;
+    ROcp31_17 = ROcp31_16*C7-S5*S7;
+    ROcp31_27 = ROcp31_26*C7-ROcp31_85*S7;
+    ROcp31_37 = ROcp31_36*C7-ROcp31_95*S7;
+    ROcp31_77 = ROcp31_16*S7+S5*C7;
+    ROcp31_87 = ROcp31_26*S7+ROcp31_85*C7;
+    ROcp31_97 = ROcp31_36*S7+ROcp31_95*C7;
+    ROcp31_48 = ROcp31_46*C8+ROcp31_77*S8;
+    ROcp31_58 = ROcp31_56*C8+ROcp31_87*S8;
+    ROcp31_68 = ROcp31_66*C8+ROcp31_97*S8;
+    ROcp31_78 = -ROcp31_46*S8+ROcp31_77*C8;
+    ROcp31_88 = -ROcp31_56*S8+ROcp31_87*C8;
+    ROcp31_98 = -ROcp31_66*S8+ROcp31_97*C8;
+    ROcp31_19 = ROcp31_17*C9+ROcp31_48*S9;
+    ROcp31_29 = ROcp31_27*C9+ROcp31_58*S9;
+    ROcp31_39 = ROcp31_37*C9+ROcp31_68*S9;
+    ROcp31_49 = -ROcp31_17*S9+ROcp31_48*C9;
+    ROcp31_59 = -ROcp31_27*S9+ROcp31_58*C9;
+    ROcp31_69 = -ROcp31_37*S9+ROcp31_68*C9;
+    ROcp31_110 = ROcp31_19*C10-ROcp31_78*S10;
+    ROcp31_210 = ROcp31_29*C10-ROcp31_88*S10;
+    ROcp31_310 = ROcp31_39*C10-ROcp31_98*S10;
+    ROcp31_710 = ROcp31_19*S10+ROcp31_78*C10;
+    ROcp31_810 = ROcp31_29*S10+ROcp31_88*C10;
+    ROcp31_910 = ROcp31_39*S10+ROcp31_98*C10;
+    ROcp31_411 = ROcp31_49*C11+ROcp31_710*S11;
+    ROcp31_511 = ROcp31_59*C11+ROcp31_810*S11;
+    ROcp31_611 = ROcp31_69*C11+ROcp31_910*S11;
+    ROcp31_711 = -ROcp31_49*S11+ROcp31_710*C11;
+    ROcp31_811 = -ROcp31_59*S11+ROcp31_810*C11;
+    ROcp31_911 = -ROcp31_69*S11+ROcp31_910*C11;
+    ROcp31_112 = ROcp31_110*C12-ROcp31_711*S12;
+    ROcp31_212 = ROcp31_210*C12-ROcp31_811*S12;
+    ROcp31_312 = ROcp31_310*C12-ROcp31_911*S12;
+    ROcp31_712 = ROcp31_110*S12+ROcp31_711*C12;
+    ROcp31_812 = ROcp31_210*S12+ROcp31_811*C12;
+    ROcp31_912 = ROcp31_310*S12+ROcp31_911*C12;
+    OMcp31_25 = qd[5]*C4;
+    OMcp31_35 = qd[5]*S4;
+    OMcp31_16 = qd[4]+qd[6]*S5;
+    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
+    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
+    RLcp31_17 = ROcp31_46*dpt[2][2];
+    RLcp31_27 = ROcp31_56*dpt[2][2];
+    RLcp31_37 = ROcp31_66*dpt[2][2];
+    POcp31_17 = q[1]+RLcp31_17;
+    POcp31_27 = q[2]+RLcp31_27;
+    POcp31_37 = q[3]+RLcp31_37;
+    OMcp31_17 = OMcp31_16+qd[7]*ROcp31_46;
+    OMcp31_27 = OMcp31_26+qd[7]*ROcp31_56;
+    OMcp31_37 = OMcp31_36+qd[7]*ROcp31_66;
+    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
+    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
+    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
+    VIcp31_17 = qd[1]+ORcp31_17;
+    VIcp31_27 = qd[2]+ORcp31_27;
+    VIcp31_37 = qd[3]+ORcp31_37;
+    RLcp31_18 = ROcp31_46*dpt[2][6];
+    RLcp31_28 = ROcp31_56*dpt[2][6];
+    RLcp31_38 = ROcp31_66*dpt[2][6];
+    POcp31_18 = POcp31_17+RLcp31_18;
+    POcp31_28 = POcp31_27+RLcp31_28;
+    POcp31_38 = POcp31_37+RLcp31_38;
+    OMcp31_18 = OMcp31_17+qd[8]*ROcp31_17;
+    OMcp31_28 = OMcp31_27+qd[8]*ROcp31_27;
+    OMcp31_38 = OMcp31_37+qd[8]*ROcp31_37;
+    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
+    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
+    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
+    VIcp31_18 = ORcp31_18+VIcp31_17;
+    VIcp31_28 = ORcp31_28+VIcp31_27;
+    VIcp31_38 = ORcp31_38+VIcp31_37;
+    RLcp31_19 = ROcp31_78*dpt[3][8];
+    RLcp31_29 = ROcp31_88*dpt[3][8];
+    RLcp31_39 = ROcp31_98*dpt[3][8];
+    POcp31_19 = POcp31_18+RLcp31_19;
+    POcp31_29 = POcp31_28+RLcp31_29;
+    POcp31_39 = POcp31_38+RLcp31_39;
+    OMcp31_19 = OMcp31_18+qd[9]*ROcp31_78;
+    OMcp31_29 = OMcp31_28+qd[9]*ROcp31_88;
+    OMcp31_39 = OMcp31_38+qd[9]*ROcp31_98;
+    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
+    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
+    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
+    VIcp31_19 = ORcp31_19+VIcp31_18;
+    VIcp31_29 = ORcp31_29+VIcp31_28;
+    VIcp31_39 = ORcp31_39+VIcp31_38;
+    RLcp31_110 = ROcp31_78*dpt[3][11];
+    RLcp31_210 = ROcp31_88*dpt[3][11];
+    RLcp31_310 = ROcp31_98*dpt[3][11];
+    POcp31_110 = POcp31_19+RLcp31_110;
+    POcp31_210 = POcp31_29+RLcp31_210;
+    POcp31_310 = POcp31_39+RLcp31_310;
+    OMcp31_110 = OMcp31_19+qd[10]*ROcp31_49;
+    OMcp31_210 = OMcp31_29+qd[10]*ROcp31_59;
+    OMcp31_310 = OMcp31_39+qd[10]*ROcp31_69;
+    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
+    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
+    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
+    VIcp31_110 = ORcp31_110+VIcp31_19;
+    VIcp31_210 = ORcp31_210+VIcp31_29;
+    VIcp31_310 = ORcp31_310+VIcp31_39;
+    RLcp31_111 = ROcp31_710*dpt[3][14];
+    RLcp31_211 = ROcp31_810*dpt[3][14];
+    RLcp31_311 = ROcp31_910*dpt[3][14];
+    POcp31_111 = POcp31_110+RLcp31_111;
+    POcp31_211 = POcp31_210+RLcp31_211;
+    POcp31_311 = POcp31_310+RLcp31_311;
+    OMcp31_111 = OMcp31_110+qd[11]*ROcp31_110;
+    OMcp31_211 = OMcp31_210+qd[11]*ROcp31_210;
+    OMcp31_311 = OMcp31_310+qd[11]*ROcp31_310;
+    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
+    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
+    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
+    VIcp31_111 = ORcp31_111+VIcp31_110;
+    VIcp31_211 = ORcp31_211+VIcp31_210;
+    VIcp31_311 = ORcp31_311+VIcp31_310;
+    OMcp31_112 = OMcp31_111+qd[12]*ROcp31_411;
+    OMcp31_212 = OMcp31_211+qd[12]*ROcp31_511;
+    OMcp31_312 = OMcp31_311+qd[12]*ROcp31_611;
+    RLcp31_113 = ROcp31_112*dpt[1][21]+ROcp31_712*Dz143;
+    RLcp31_213 = ROcp31_212*dpt[1][21]+ROcp31_812*Dz143;
+    RLcp31_313 = ROcp31_312*dpt[1][21]+ROcp31_912*Dz143;
+    POcp31_113 = POcp31_111+RLcp31_113;
+    POcp31_213 = POcp31_211+RLcp31_213;
+    POcp31_313 = POcp31_311+RLcp31_313;
+    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
+    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
+    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
+    VIcp31_113 = ORcp31_113+VIcp31_111+qd[14]*ROcp31_712;
+    VIcp31_213 = ORcp31_213+VIcp31_211+qd[14]*ROcp31_812;
+    VIcp31_313 = ORcp31_313+VIcp31_311+qd[14]*ROcp31_912;
+    PxF5[1] = POcp31_113;
+    PxF5[2] = POcp31_213;
+    PxF5[3] = POcp31_313;
+    RxF5[1][1] = ROcp31_112;
+    RxF5[1][2] = ROcp31_212;
+    RxF5[1][3] = ROcp31_312;
+    RxF5[2][1] = ROcp31_411;
+    RxF5[2][2] = ROcp31_511;
+    RxF5[2][3] = ROcp31_611;
+    RxF5[3][1] = ROcp31_712;
+    RxF5[3][2] = ROcp31_812;
+    RxF5[3][3] = ROcp31_912;
+    VxF5[1] = VIcp31_113;
+    VxF5[2] = VIcp31_213;
+    VxF5[3] = VIcp31_313;
+    OMxF5[1] = OMcp31_112;
+    OMxF5[2] = OMcp31_212;
+    OMxF5[3] = OMcp31_312;
+    ROcp32_25 = S4*S5;
+    ROcp32_35 = -C4*S5;
+    ROcp32_85 = -S4*C5;
+    ROcp32_95 = C4*C5;
+    ROcp32_16 = C5*C6;
+    ROcp32_26 = ROcp32_25*C6+C4*S6;
+    ROcp32_36 = ROcp32_35*C6+S4*S6;
+    ROcp32_46 = -C5*S6;
+    ROcp32_56 = -ROcp32_25*S6+C4*C6;
+    ROcp32_66 = -ROcp32_35*S6+S4*C6;
+    ROcp32_115 = ROcp32_16*C15-S15*S5;
+    ROcp32_215 = ROcp32_26*C15-ROcp32_85*S15;
+    ROcp32_315 = ROcp32_36*C15-ROcp32_95*S15;
+    ROcp32_715 = ROcp32_16*S15+C15*S5;
+    ROcp32_815 = ROcp32_26*S15+ROcp32_85*C15;
+    ROcp32_915 = ROcp32_36*S15+ROcp32_95*C15;
+    ROcp32_416 = ROcp32_46*C16+ROcp32_715*S16;
+    ROcp32_516 = ROcp32_56*C16+ROcp32_815*S16;
+    ROcp32_616 = ROcp32_66*C16+ROcp32_915*S16;
+    ROcp32_716 = -ROcp32_46*S16+ROcp32_715*C16;
+    ROcp32_816 = -ROcp32_56*S16+ROcp32_815*C16;
+    ROcp32_916 = -ROcp32_66*S16+ROcp32_915*C16;
+    ROcp32_117 = ROcp32_115*C17+ROcp32_416*S17;
+    ROcp32_217 = ROcp32_215*C17+ROcp32_516*S17;
+    ROcp32_317 = ROcp32_315*C17+ROcp32_616*S17;
+    ROcp32_417 = -ROcp32_115*S17+ROcp32_416*C17;
+    ROcp32_517 = -ROcp32_215*S17+ROcp32_516*C17;
+    ROcp32_617 = -ROcp32_315*S17+ROcp32_616*C17;
+    OMcp32_25 = qd[5]*C4;
+    OMcp32_35 = qd[5]*S4;
+    OMcp32_16 = qd[4]+qd[6]*S5;
+    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
+    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
+    RLcp32_17 = ROcp32_46*dpt[2][3];
+    RLcp32_27 = ROcp32_56*dpt[2][3];
+    RLcp32_37 = ROcp32_66*dpt[2][3];
+    POcp32_17 = q[1]+RLcp32_17;
+    POcp32_27 = q[2]+RLcp32_27;
+    POcp32_37 = q[3]+RLcp32_37;
+    OMcp32_17 = OMcp32_16+qd[15]*ROcp32_46;
+    OMcp32_27 = OMcp32_26+qd[15]*ROcp32_56;
+    OMcp32_37 = OMcp32_36+qd[15]*ROcp32_66;
+    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
+    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
+    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
+    VIcp32_17 = qd[1]+ORcp32_17;
+    VIcp32_27 = qd[2]+ORcp32_27;
+    VIcp32_37 = qd[3]+ORcp32_37;
+    RLcp32_18 = ROcp32_46*dpt[2][24];
+    RLcp32_28 = ROcp32_56*dpt[2][24];
+    RLcp32_38 = ROcp32_66*dpt[2][24];
+    POcp32_18 = POcp32_17+RLcp32_18;
+    POcp32_28 = POcp32_27+RLcp32_28;
+    POcp32_38 = POcp32_37+RLcp32_38;
+    OMcp32_18 = OMcp32_17+qd[16]*ROcp32_115;
+    OMcp32_28 = OMcp32_27+qd[16]*ROcp32_215;
+    OMcp32_38 = OMcp32_37+qd[16]*ROcp32_315;
+    ORcp32_18 = OMcp32_27*RLcp32_38-OMcp32_37*RLcp32_28;
+    ORcp32_28 = -OMcp32_17*RLcp32_38+OMcp32_37*RLcp32_18;
+    ORcp32_38 = OMcp32_17*RLcp32_28-OMcp32_27*RLcp32_18;
+    VIcp32_18 = ORcp32_18+VIcp32_17;
+    VIcp32_28 = ORcp32_28+VIcp32_27;
+    VIcp32_38 = ORcp32_38+VIcp32_37;
+    RLcp32_19 = ROcp32_716*dpt[3][26];
+    RLcp32_29 = ROcp32_816*dpt[3][26];
+    RLcp32_39 = ROcp32_916*dpt[3][26];
+    POcp32_19 = POcp32_18+RLcp32_19;
+    POcp32_29 = POcp32_28+RLcp32_29;
+    POcp32_39 = POcp32_38+RLcp32_39;
+    OMcp32_19 = OMcp32_18+qd[17]*ROcp32_716;
+    OMcp32_29 = OMcp32_28+qd[17]*ROcp32_816;
+    OMcp32_39 = OMcp32_38+qd[17]*ROcp32_916;
+    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
+    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
+    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
+    VIcp32_19 = ORcp32_19+VIcp32_18;
+    VIcp32_29 = ORcp32_29+VIcp32_28;
+    VIcp32_39 = ORcp32_39+VIcp32_38;
+    PxF6[1] = POcp32_19;
+    PxF6[2] = POcp32_29;
+    PxF6[3] = POcp32_39;
+    RxF6[1][1] = ROcp32_117;
+    RxF6[1][2] = ROcp32_217;
+    RxF6[1][3] = ROcp32_317;
+    RxF6[2][1] = ROcp32_417;
+    RxF6[2][2] = ROcp32_517;
+    RxF6[2][3] = ROcp32_617;
+    RxF6[3][1] = ROcp32_716;
+    RxF6[3][2] = ROcp32_816;
+    RxF6[3][3] = ROcp32_916;
+    VxF6[1] = VIcp32_19;
+    VxF6[2] = VIcp32_29;
+    VxF6[3] = VIcp32_39;
+    OMxF6[1] = OMcp32_19;
+    OMxF6[2] = OMcp32_29;
+    OMxF6[3] = OMcp32_39;
+    ROcp33_25 = S4*S5;
+    ROcp33_35 = -C4*S5;
+    ROcp33_85 = -S4*C5;
+    ROcp33_95 = C4*C5;
+    ROcp33_16 = C5*C6;
+    ROcp33_26 = ROcp33_25*C6+C4*S6;
+    ROcp33_36 = ROcp33_35*C6+S4*S6;
+    ROcp33_46 = -C5*S6;
+    ROcp33_56 = -ROcp33_25*S6+C4*C6;
+    ROcp33_66 = -ROcp33_35*S6+S4*C6;
+    ROcp33_115 = ROcp33_16*C15-S15*S5;
+    ROcp33_215 = ROcp33_26*C15-ROcp33_85*S15;
+    ROcp33_315 = ROcp33_36*C15-ROcp33_95*S15;
+    ROcp33_715 = ROcp33_16*S15+C15*S5;
+    ROcp33_815 = ROcp33_26*S15+ROcp33_85*C15;
+    ROcp33_915 = ROcp33_36*S15+ROcp33_95*C15;
+    ROcp33_416 = ROcp33_46*C16+ROcp33_715*S16;
+    ROcp33_516 = ROcp33_56*C16+ROcp33_815*S16;
+    ROcp33_616 = ROcp33_66*C16+ROcp33_915*S16;
+    ROcp33_716 = -ROcp33_46*S16+ROcp33_715*C16;
+    ROcp33_816 = -ROcp33_56*S16+ROcp33_815*C16;
+    ROcp33_916 = -ROcp33_66*S16+ROcp33_915*C16;
+    ROcp33_117 = ROcp33_115*C17+ROcp33_416*S17;
+    ROcp33_217 = ROcp33_215*C17+ROcp33_516*S17;
+    ROcp33_317 = ROcp33_315*C17+ROcp33_616*S17;
+    ROcp33_417 = -ROcp33_115*S17+ROcp33_416*C17;
+    ROcp33_517 = -ROcp33_215*S17+ROcp33_516*C17;
+    ROcp33_617 = -ROcp33_315*S17+ROcp33_616*C17;
+    ROcp33_118 = ROcp33_117*C18-ROcp33_716*S18;
+    ROcp33_218 = ROcp33_217*C18-ROcp33_816*S18;
+    ROcp33_318 = ROcp33_317*C18-ROcp33_916*S18;
+    ROcp33_718 = ROcp33_117*S18+ROcp33_716*C18;
+    ROcp33_818 = ROcp33_217*S18+ROcp33_816*C18;
+    ROcp33_918 = ROcp33_317*S18+ROcp33_916*C18;
+    OMcp33_25 = qd[5]*C4;
+    OMcp33_35 = qd[5]*S4;
+    OMcp33_16 = qd[4]+qd[6]*S5;
+    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
+    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
+    RLcp33_17 = ROcp33_46*dpt[2][3];
+    RLcp33_27 = ROcp33_56*dpt[2][3];
+    RLcp33_37 = ROcp33_66*dpt[2][3];
+    POcp33_17 = q[1]+RLcp33_17;
+    POcp33_27 = q[2]+RLcp33_27;
+    POcp33_37 = q[3]+RLcp33_37;
+    OMcp33_17 = OMcp33_16+qd[15]*ROcp33_46;
+    OMcp33_27 = OMcp33_26+qd[15]*ROcp33_56;
+    OMcp33_37 = OMcp33_36+qd[15]*ROcp33_66;
+    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
+    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
+    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
+    VIcp33_17 = qd[1]+ORcp33_17;
+    VIcp33_27 = qd[2]+ORcp33_27;
+    VIcp33_37 = qd[3]+ORcp33_37;
+    RLcp33_18 = ROcp33_46*dpt[2][24];
+    RLcp33_28 = ROcp33_56*dpt[2][24];
+    RLcp33_38 = ROcp33_66*dpt[2][24];
+    POcp33_18 = POcp33_17+RLcp33_18;
+    POcp33_28 = POcp33_27+RLcp33_28;
+    POcp33_38 = POcp33_37+RLcp33_38;
+    OMcp33_18 = OMcp33_17+qd[16]*ROcp33_115;
+    OMcp33_28 = OMcp33_27+qd[16]*ROcp33_215;
+    OMcp33_38 = OMcp33_37+qd[16]*ROcp33_315;
+    ORcp33_18 = OMcp33_27*RLcp33_38-OMcp33_37*RLcp33_28;
+    ORcp33_28 = -OMcp33_17*RLcp33_38+OMcp33_37*RLcp33_18;
+    ORcp33_38 = OMcp33_17*RLcp33_28-OMcp33_27*RLcp33_18;
+    VIcp33_18 = ORcp33_18+VIcp33_17;
+    VIcp33_28 = ORcp33_28+VIcp33_27;
+    VIcp33_38 = ORcp33_38+VIcp33_37;
+    RLcp33_19 = ROcp33_716*dpt[3][26];
+    RLcp33_29 = ROcp33_816*dpt[3][26];
+    RLcp33_39 = ROcp33_916*dpt[3][26];
+    POcp33_19 = POcp33_18+RLcp33_19;
+    POcp33_29 = POcp33_28+RLcp33_29;
+    POcp33_39 = POcp33_38+RLcp33_39;
+    OMcp33_19 = OMcp33_18+qd[17]*ROcp33_716;
+    OMcp33_29 = OMcp33_28+qd[17]*ROcp33_816;
+    OMcp33_39 = OMcp33_38+qd[17]*ROcp33_916;
+    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
+    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
+    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
+    VIcp33_19 = ORcp33_19+VIcp33_18;
+    VIcp33_29 = ORcp33_29+VIcp33_28;
+    VIcp33_39 = ORcp33_39+VIcp33_38;
+    RLcp33_110 = ROcp33_716*dpt[3][29];
+    RLcp33_210 = ROcp33_816*dpt[3][29];
+    RLcp33_310 = ROcp33_916*dpt[3][29];
+    POcp33_110 = POcp33_19+RLcp33_110;
+    POcp33_210 = POcp33_29+RLcp33_210;
+    POcp33_310 = POcp33_39+RLcp33_310;
+    OMcp33_110 = OMcp33_19+qd[18]*ROcp33_417;
+    OMcp33_210 = OMcp33_29+qd[18]*ROcp33_517;
+    OMcp33_310 = OMcp33_39+qd[18]*ROcp33_617;
+    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
+    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
+    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
+    VIcp33_110 = ORcp33_110+VIcp33_19;
+    VIcp33_210 = ORcp33_210+VIcp33_29;
+    VIcp33_310 = ORcp33_310+VIcp33_39;
+    PxF7[1] = POcp33_110;
+    PxF7[2] = POcp33_210;
+    PxF7[3] = POcp33_310;
+    RxF7[1][1] = ROcp33_118;
+    RxF7[1][2] = ROcp33_218;
+    RxF7[1][3] = ROcp33_318;
+    RxF7[2][1] = ROcp33_417;
+    RxF7[2][2] = ROcp33_517;
+    RxF7[2][3] = ROcp33_617;
+    RxF7[3][1] = ROcp33_718;
+    RxF7[3][2] = ROcp33_818;
+    RxF7[3][3] = ROcp33_918;
+    VxF7[1] = VIcp33_110;
+    VxF7[2] = VIcp33_210;
+    VxF7[3] = VIcp33_310;
+    OMxF7[1] = OMcp33_110;
+    OMxF7[2] = OMcp33_210;
+    OMxF7[3] = OMcp33_310;
+    ROcp34_25 = S4*S5;
+    ROcp34_35 = -C4*S5;
+    ROcp34_85 = -S4*C5;
+    ROcp34_95 = C4*C5;
+    ROcp34_16 = C5*C6;
+    ROcp34_26 = ROcp34_25*C6+C4*S6;
+    ROcp34_36 = ROcp34_35*C6+S4*S6;
+    ROcp34_46 = -C5*S6;
+    ROcp34_56 = -ROcp34_25*S6+C4*C6;
+    ROcp34_66 = -ROcp34_35*S6+S4*C6;
+    ROcp34_115 = ROcp34_16*C15-S15*S5;
+    ROcp34_215 = ROcp34_26*C15-ROcp34_85*S15;
+    ROcp34_315 = ROcp34_36*C15-ROcp34_95*S15;
+    ROcp34_715 = ROcp34_16*S15+C15*S5;
+    ROcp34_815 = ROcp34_26*S15+ROcp34_85*C15;
+    ROcp34_915 = ROcp34_36*S15+ROcp34_95*C15;
+    ROcp34_416 = ROcp34_46*C16+ROcp34_715*S16;
+    ROcp34_516 = ROcp34_56*C16+ROcp34_815*S16;
+    ROcp34_616 = ROcp34_66*C16+ROcp34_915*S16;
+    ROcp34_716 = -ROcp34_46*S16+ROcp34_715*C16;
+    ROcp34_816 = -ROcp34_56*S16+ROcp34_815*C16;
+    ROcp34_916 = -ROcp34_66*S16+ROcp34_915*C16;
+    ROcp34_117 = ROcp34_115*C17+ROcp34_416*S17;
+    ROcp34_217 = ROcp34_215*C17+ROcp34_516*S17;
+    ROcp34_317 = ROcp34_315*C17+ROcp34_616*S17;
+    ROcp34_417 = -ROcp34_115*S17+ROcp34_416*C17;
+    ROcp34_517 = -ROcp34_215*S17+ROcp34_516*C17;
+    ROcp34_617 = -ROcp34_315*S17+ROcp34_616*C17;
+    ROcp34_118 = ROcp34_117*C18-ROcp34_716*S18;
+    ROcp34_218 = ROcp34_217*C18-ROcp34_816*S18;
+    ROcp34_318 = ROcp34_317*C18-ROcp34_916*S18;
+    ROcp34_718 = ROcp34_117*S18+ROcp34_716*C18;
+    ROcp34_818 = ROcp34_217*S18+ROcp34_816*C18;
+    ROcp34_918 = ROcp34_317*S18+ROcp34_916*C18;
+    ROcp34_419 = ROcp34_417*C19+ROcp34_718*S19;
+    ROcp34_519 = ROcp34_517*C19+ROcp34_818*S19;
+    ROcp34_619 = ROcp34_617*C19+ROcp34_918*S19;
+    ROcp34_719 = -ROcp34_417*S19+ROcp34_718*C19;
+    ROcp34_819 = -ROcp34_517*S19+ROcp34_818*C19;
+    ROcp34_919 = -ROcp34_617*S19+ROcp34_918*C19;
+    ROcp34_120 = ROcp34_118*C20-ROcp34_719*S20;
+    ROcp34_220 = ROcp34_218*C20-ROcp34_819*S20;
+    ROcp34_320 = ROcp34_318*C20-ROcp34_919*S20;
+    ROcp34_720 = ROcp34_118*S20+ROcp34_719*C20;
+    ROcp34_820 = ROcp34_218*S20+ROcp34_819*C20;
+    ROcp34_920 = ROcp34_318*S20+ROcp34_919*C20;
+    OMcp34_25 = qd[5]*C4;
+    OMcp34_35 = qd[5]*S4;
+    OMcp34_16 = qd[4]+qd[6]*S5;
+    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
+    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
+    RLcp34_17 = ROcp34_46*dpt[2][3];
+    RLcp34_27 = ROcp34_56*dpt[2][3];
+    RLcp34_37 = ROcp34_66*dpt[2][3];
+    POcp34_17 = q[1]+RLcp34_17;
+    POcp34_27 = q[2]+RLcp34_27;
+    POcp34_37 = q[3]+RLcp34_37;
+    OMcp34_17 = OMcp34_16+qd[15]*ROcp34_46;
+    OMcp34_27 = OMcp34_26+qd[15]*ROcp34_56;
+    OMcp34_37 = OMcp34_36+qd[15]*ROcp34_66;
+    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
+    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
+    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
+    VIcp34_17 = qd[1]+ORcp34_17;
+    VIcp34_27 = qd[2]+ORcp34_27;
+    VIcp34_37 = qd[3]+ORcp34_37;
+    RLcp34_18 = ROcp34_46*dpt[2][24];
+    RLcp34_28 = ROcp34_56*dpt[2][24];
+    RLcp34_38 = ROcp34_66*dpt[2][24];
+    POcp34_18 = POcp34_17+RLcp34_18;
+    POcp34_28 = POcp34_27+RLcp34_28;
+    POcp34_38 = POcp34_37+RLcp34_38;
+    OMcp34_18 = OMcp34_17+qd[16]*ROcp34_115;
+    OMcp34_28 = OMcp34_27+qd[16]*ROcp34_215;
+    OMcp34_38 = OMcp34_37+qd[16]*ROcp34_315;
+    ORcp34_18 = OMcp34_27*RLcp34_38-OMcp34_37*RLcp34_28;
+    ORcp34_28 = -OMcp34_17*RLcp34_38+OMcp34_37*RLcp34_18;
+    ORcp34_38 = OMcp34_17*RLcp34_28-OMcp34_27*RLcp34_18;
+    VIcp34_18 = ORcp34_18+VIcp34_17;
+    VIcp34_28 = ORcp34_28+VIcp34_27;
+    VIcp34_38 = ORcp34_38+VIcp34_37;
+    RLcp34_19 = ROcp34_716*dpt[3][26];
+    RLcp34_29 = ROcp34_816*dpt[3][26];
+    RLcp34_39 = ROcp34_916*dpt[3][26];
+    POcp34_19 = POcp34_18+RLcp34_19;
+    POcp34_29 = POcp34_28+RLcp34_29;
+    POcp34_39 = POcp34_38+RLcp34_39;
+    OMcp34_19 = OMcp34_18+qd[17]*ROcp34_716;
+    OMcp34_29 = OMcp34_28+qd[17]*ROcp34_816;
+    OMcp34_39 = OMcp34_38+qd[17]*ROcp34_916;
+    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
+    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
+    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
+    VIcp34_19 = ORcp34_19+VIcp34_18;
+    VIcp34_29 = ORcp34_29+VIcp34_28;
+    VIcp34_39 = ORcp34_39+VIcp34_38;
+    RLcp34_110 = ROcp34_716*dpt[3][29];
+    RLcp34_210 = ROcp34_816*dpt[3][29];
+    RLcp34_310 = ROcp34_916*dpt[3][29];
+    POcp34_110 = POcp34_19+RLcp34_110;
+    POcp34_210 = POcp34_29+RLcp34_210;
+    POcp34_310 = POcp34_39+RLcp34_310;
+    OMcp34_110 = OMcp34_19+qd[18]*ROcp34_417;
+    OMcp34_210 = OMcp34_29+qd[18]*ROcp34_517;
+    OMcp34_310 = OMcp34_39+qd[18]*ROcp34_617;
+    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
+    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
+    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
+    VIcp34_110 = ORcp34_110+VIcp34_19;
+    VIcp34_210 = ORcp34_210+VIcp34_29;
+    VIcp34_310 = ORcp34_310+VIcp34_39;
+    RLcp34_111 = ROcp34_718*dpt[3][32];
+    RLcp34_211 = ROcp34_818*dpt[3][32];
+    RLcp34_311 = ROcp34_918*dpt[3][32];
+    POcp34_111 = POcp34_110+RLcp34_111;
+    POcp34_211 = POcp34_210+RLcp34_211;
+    POcp34_311 = POcp34_310+RLcp34_311;
+    OMcp34_111 = OMcp34_110+qd[19]*ROcp34_118;
+    OMcp34_211 = OMcp34_210+qd[19]*ROcp34_218;
+    OMcp34_311 = OMcp34_310+qd[19]*ROcp34_318;
+    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
+    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
+    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
+    VIcp34_111 = ORcp34_111+VIcp34_110;
+    VIcp34_211 = ORcp34_211+VIcp34_210;
+    VIcp34_311 = ORcp34_311+VIcp34_310;
+    OMcp34_112 = OMcp34_111+qd[20]*ROcp34_419;
+    OMcp34_212 = OMcp34_211+qd[20]*ROcp34_519;
+    OMcp34_312 = OMcp34_311+qd[20]*ROcp34_619;
+    RLcp34_113 = ROcp34_120*dpt[1][38]+ROcp34_720*Dz213;
+    RLcp34_213 = ROcp34_220*dpt[1][38]+ROcp34_820*Dz213;
+    RLcp34_313 = ROcp34_320*dpt[1][38]+ROcp34_920*Dz213;
+    POcp34_113 = POcp34_111+RLcp34_113;
+    POcp34_213 = POcp34_211+RLcp34_213;
+    POcp34_313 = POcp34_311+RLcp34_313;
+    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
+    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
+    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
+    VIcp34_113 = ORcp34_113+VIcp34_111+qd[21]*ROcp34_720;
+    VIcp34_213 = ORcp34_213+VIcp34_211+qd[21]*ROcp34_820;
+    VIcp34_313 = ORcp34_313+VIcp34_311+qd[21]*ROcp34_920;
+    PxF8[1] = POcp34_113;
+    PxF8[2] = POcp34_213;
+    PxF8[3] = POcp34_313;
+    RxF8[1][1] = ROcp34_120;
+    RxF8[1][2] = ROcp34_220;
+    RxF8[1][3] = ROcp34_320;
+    RxF8[2][1] = ROcp34_419;
+    RxF8[2][2] = ROcp34_519;
+    RxF8[2][3] = ROcp34_619;
+    RxF8[3][1] = ROcp34_720;
+    RxF8[3][2] = ROcp34_820;
+    RxF8[3][3] = ROcp34_920;
+    VxF8[1] = VIcp34_113;
+    VxF8[2] = VIcp34_213;
+    VxF8[3] = VIcp34_313;
+    OMxF8[1] = OMcp34_112;
+    OMxF8[2] = OMcp34_212;
+    OMxF8[3] = OMcp34_312;
+    ROcp35_25 = S4*S5;
+    ROcp35_35 = -C4*S5;
+    ROcp35_85 = -S4*C5;
+    ROcp35_95 = C4*C5;
+    ROcp35_16 = C5*C6;
+    ROcp35_26 = ROcp35_25*C6+C4*S6;
+    ROcp35_36 = ROcp35_35*C6+S4*S6;
+    ROcp35_46 = -C5*S6;
+    ROcp35_56 = -ROcp35_25*S6+C4*C6;
+    ROcp35_66 = -ROcp35_35*S6+S4*C6;
+    ROcp35_115 = ROcp35_16*C15-S15*S5;
+    ROcp35_215 = ROcp35_26*C15-ROcp35_85*S15;
+    ROcp35_315 = ROcp35_36*C15-ROcp35_95*S15;
+    ROcp35_715 = ROcp35_16*S15+C15*S5;
+    ROcp35_815 = ROcp35_26*S15+ROcp35_85*C15;
+    ROcp35_915 = ROcp35_36*S15+ROcp35_95*C15;
+    ROcp35_416 = ROcp35_46*C16+ROcp35_715*S16;
+    ROcp35_516 = ROcp35_56*C16+ROcp35_815*S16;
+    ROcp35_616 = ROcp35_66*C16+ROcp35_915*S16;
+    ROcp35_716 = -ROcp35_46*S16+ROcp35_715*C16;
+    ROcp35_816 = -ROcp35_56*S16+ROcp35_815*C16;
+    ROcp35_916 = -ROcp35_66*S16+ROcp35_915*C16;
+    ROcp35_117 = ROcp35_115*C17+ROcp35_416*S17;
+    ROcp35_217 = ROcp35_215*C17+ROcp35_516*S17;
+    ROcp35_317 = ROcp35_315*C17+ROcp35_616*S17;
+    ROcp35_417 = -ROcp35_115*S17+ROcp35_416*C17;
+    ROcp35_517 = -ROcp35_215*S17+ROcp35_516*C17;
+    ROcp35_617 = -ROcp35_315*S17+ROcp35_616*C17;
+    ROcp35_118 = ROcp35_117*C18-ROcp35_716*S18;
+    ROcp35_218 = ROcp35_217*C18-ROcp35_816*S18;
+    ROcp35_318 = ROcp35_317*C18-ROcp35_916*S18;
+    ROcp35_718 = ROcp35_117*S18+ROcp35_716*C18;
+    ROcp35_818 = ROcp35_217*S18+ROcp35_816*C18;
+    ROcp35_918 = ROcp35_317*S18+ROcp35_916*C18;
+    ROcp35_419 = ROcp35_417*C19+ROcp35_718*S19;
+    ROcp35_519 = ROcp35_517*C19+ROcp35_818*S19;
+    ROcp35_619 = ROcp35_617*C19+ROcp35_918*S19;
+    ROcp35_719 = -ROcp35_417*S19+ROcp35_718*C19;
+    ROcp35_819 = -ROcp35_517*S19+ROcp35_818*C19;
+    ROcp35_919 = -ROcp35_617*S19+ROcp35_918*C19;
+    ROcp35_120 = ROcp35_118*C20-ROcp35_719*S20;
+    ROcp35_220 = ROcp35_218*C20-ROcp35_819*S20;
+    ROcp35_320 = ROcp35_318*C20-ROcp35_919*S20;
+    ROcp35_720 = ROcp35_118*S20+ROcp35_719*C20;
+    ROcp35_820 = ROcp35_218*S20+ROcp35_819*C20;
+    ROcp35_920 = ROcp35_318*S20+ROcp35_919*C20;
+    OMcp35_25 = qd[5]*C4;
+    OMcp35_35 = qd[5]*S4;
+    OMcp35_16 = qd[4]+qd[6]*S5;
+    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
+    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
+    RLcp35_17 = ROcp35_46*dpt[2][3];
+    RLcp35_27 = ROcp35_56*dpt[2][3];
+    RLcp35_37 = ROcp35_66*dpt[2][3];
+    POcp35_17 = q[1]+RLcp35_17;
+    POcp35_27 = q[2]+RLcp35_27;
+    POcp35_37 = q[3]+RLcp35_37;
+    OMcp35_17 = OMcp35_16+qd[15]*ROcp35_46;
+    OMcp35_27 = OMcp35_26+qd[15]*ROcp35_56;
+    OMcp35_37 = OMcp35_36+qd[15]*ROcp35_66;
+    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
+    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
+    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
+    VIcp35_17 = qd[1]+ORcp35_17;
+    VIcp35_27 = qd[2]+ORcp35_27;
+    VIcp35_37 = qd[3]+ORcp35_37;
+    RLcp35_18 = ROcp35_46*dpt[2][24];
+    RLcp35_28 = ROcp35_56*dpt[2][24];
+    RLcp35_38 = ROcp35_66*dpt[2][24];
+    POcp35_18 = POcp35_17+RLcp35_18;
+    POcp35_28 = POcp35_27+RLcp35_28;
+    POcp35_38 = POcp35_37+RLcp35_38;
+    OMcp35_18 = OMcp35_17+qd[16]*ROcp35_115;
+    OMcp35_28 = OMcp35_27+qd[16]*ROcp35_215;
+    OMcp35_38 = OMcp35_37+qd[16]*ROcp35_315;
+    ORcp35_18 = OMcp35_27*RLcp35_38-OMcp35_37*RLcp35_28;
+    ORcp35_28 = -OMcp35_17*RLcp35_38+OMcp35_37*RLcp35_18;
+    ORcp35_38 = OMcp35_17*RLcp35_28-OMcp35_27*RLcp35_18;
+    VIcp35_18 = ORcp35_18+VIcp35_17;
+    VIcp35_28 = ORcp35_28+VIcp35_27;
+    VIcp35_38 = ORcp35_38+VIcp35_37;
+    RLcp35_19 = ROcp35_716*dpt[3][26];
+    RLcp35_29 = ROcp35_816*dpt[3][26];
+    RLcp35_39 = ROcp35_916*dpt[3][26];
+    POcp35_19 = POcp35_18+RLcp35_19;
+    POcp35_29 = POcp35_28+RLcp35_29;
+    POcp35_39 = POcp35_38+RLcp35_39;
+    OMcp35_19 = OMcp35_18+qd[17]*ROcp35_716;
+    OMcp35_29 = OMcp35_28+qd[17]*ROcp35_816;
+    OMcp35_39 = OMcp35_38+qd[17]*ROcp35_916;
+    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
+    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
+    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
+    VIcp35_19 = ORcp35_19+VIcp35_18;
+    VIcp35_29 = ORcp35_29+VIcp35_28;
+    VIcp35_39 = ORcp35_39+VIcp35_38;
+    RLcp35_110 = ROcp35_716*dpt[3][29];
+    RLcp35_210 = ROcp35_816*dpt[3][29];
+    RLcp35_310 = ROcp35_916*dpt[3][29];
+    POcp35_110 = POcp35_19+RLcp35_110;
+    POcp35_210 = POcp35_29+RLcp35_210;
+    POcp35_310 = POcp35_39+RLcp35_310;
+    OMcp35_110 = OMcp35_19+qd[18]*ROcp35_417;
+    OMcp35_210 = OMcp35_29+qd[18]*ROcp35_517;
+    OMcp35_310 = OMcp35_39+qd[18]*ROcp35_617;
+    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
+    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
+    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
+    VIcp35_110 = ORcp35_110+VIcp35_19;
+    VIcp35_210 = ORcp35_210+VIcp35_29;
+    VIcp35_310 = ORcp35_310+VIcp35_39;
+    RLcp35_111 = ROcp35_718*dpt[3][32];
+    RLcp35_211 = ROcp35_818*dpt[3][32];
+    RLcp35_311 = ROcp35_918*dpt[3][32];
+    POcp35_111 = POcp35_110+RLcp35_111;
+    POcp35_211 = POcp35_210+RLcp35_211;
+    POcp35_311 = POcp35_310+RLcp35_311;
+    OMcp35_111 = OMcp35_110+qd[19]*ROcp35_118;
+    OMcp35_211 = OMcp35_210+qd[19]*ROcp35_218;
+    OMcp35_311 = OMcp35_310+qd[19]*ROcp35_318;
+    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
+    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
+    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
+    VIcp35_111 = ORcp35_111+VIcp35_110;
+    VIcp35_211 = ORcp35_211+VIcp35_210;
+    VIcp35_311 = ORcp35_311+VIcp35_310;
+    OMcp35_112 = OMcp35_111+qd[20]*ROcp35_419;
+    OMcp35_212 = OMcp35_211+qd[20]*ROcp35_519;
+    OMcp35_312 = OMcp35_311+qd[20]*ROcp35_619;
+    RLcp35_113 = ROcp35_120*dpt[1][39]+ROcp35_720*Dz223;
+    RLcp35_213 = ROcp35_220*dpt[1][39]+ROcp35_820*Dz223;
+    RLcp35_313 = ROcp35_320*dpt[1][39]+ROcp35_920*Dz223;
+    POcp35_113 = POcp35_111+RLcp35_113;
+    POcp35_213 = POcp35_211+RLcp35_213;
+    POcp35_313 = POcp35_311+RLcp35_313;
+    ORcp35_113 = OMcp35_212*RLcp35_313-OMcp35_312*RLcp35_213;
+    ORcp35_213 = -OMcp35_112*RLcp35_313+OMcp35_312*RLcp35_113;
+    ORcp35_313 = OMcp35_112*RLcp35_213-OMcp35_212*RLcp35_113;
+    VIcp35_113 = ORcp35_113+VIcp35_111+qd[22]*ROcp35_720;
+    VIcp35_213 = ORcp35_213+VIcp35_211+qd[22]*ROcp35_820;
+    VIcp35_313 = ORcp35_313+VIcp35_311+qd[22]*ROcp35_920;
+    PxF9[1] = POcp35_113;
+    PxF9[2] = POcp35_213;
+    PxF9[3] = POcp35_313;
+    RxF9[1][1] = ROcp35_120;
+    RxF9[1][2] = ROcp35_220;
+    RxF9[1][3] = ROcp35_320;
+    RxF9[2][1] = ROcp35_419;
+    RxF9[2][2] = ROcp35_519;
+    RxF9[2][3] = ROcp35_619;
+    RxF9[3][1] = ROcp35_720;
+    RxF9[3][2] = ROcp35_820;
+    RxF9[3][3] = ROcp35_920;
+    VxF9[1] = VIcp35_113;
+    VxF9[2] = VIcp35_213;
+    VxF9[3] = VIcp35_313;
+    OMxF9[1] = OMcp35_112;
+    OMxF9[2] = OMcp35_212;
+    OMxF9[3] = OMcp35_312;
+    ROcp36_25 = S4*S5;
+    ROcp36_35 = -C4*S5;
+    ROcp36_85 = -S4*C5;
+    ROcp36_95 = C4*C5;
+    ROcp36_16 = C5*C6;
+    ROcp36_26 = ROcp36_25*C6+C4*S6;
+    ROcp36_36 = ROcp36_35*C6+S4*S6;
+    ROcp36_46 = -C5*S6;
+    ROcp36_56 = -ROcp36_25*S6+C4*C6;
+    ROcp36_66 = -ROcp36_35*S6+S4*C6;
+    ROcp36_423 = ROcp36_46*C23+S23*S5;
+    ROcp36_523 = ROcp36_56*C23+ROcp36_85*S23;
+    ROcp36_623 = ROcp36_66*C23+ROcp36_95*S23;
+    ROcp36_723 = -ROcp36_46*S23+C23*S5;
+    ROcp36_823 = -ROcp36_56*S23+ROcp36_85*C23;
+    ROcp36_923 = -ROcp36_66*S23+ROcp36_95*C23;
+    ROcp36_124 = ROcp36_16*C24-ROcp36_723*S24;
+    ROcp36_224 = ROcp36_26*C24-ROcp36_823*S24;
+    ROcp36_324 = ROcp36_36*C24-ROcp36_923*S24;
+    ROcp36_724 = ROcp36_16*S24+ROcp36_723*C24;
+    ROcp36_824 = ROcp36_26*S24+ROcp36_823*C24;
+    ROcp36_924 = ROcp36_36*S24+ROcp36_923*C24;
+    ROcp36_125 = ROcp36_124*C25+ROcp36_423*S25;
+    ROcp36_225 = ROcp36_224*C25+ROcp36_523*S25;
+    ROcp36_325 = ROcp36_324*C25+ROcp36_623*S25;
+    ROcp36_425 = -ROcp36_124*S25+ROcp36_423*C25;
+    ROcp36_525 = -ROcp36_224*S25+ROcp36_523*C25;
+    ROcp36_625 = -ROcp36_324*S25+ROcp36_623*C25;
+    OMcp36_25 = qd[5]*C4;
+    OMcp36_35 = qd[5]*S4;
+    OMcp36_16 = qd[4]+qd[6]*S5;
+    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
+    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
+    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
+    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
+    POcp36_17 = q[1]+RLcp36_17;
+    POcp36_27 = q[2]+RLcp36_27;
+    POcp36_37 = q[3]+RLcp36_37;
+    OMcp36_17 = OMcp36_16+qd[23]*ROcp36_16;
+    OMcp36_27 = OMcp36_26+qd[23]*ROcp36_26;
+    OMcp36_37 = OMcp36_36+qd[23]*ROcp36_36;
+    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
+    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
+    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
+    VIcp36_17 = qd[1]+ORcp36_17;
+    VIcp36_27 = qd[2]+ORcp36_27;
+    VIcp36_37 = qd[3]+ORcp36_37;
+    OMcp36_18 = OMcp36_17+qd[24]*ROcp36_423;
+    OMcp36_28 = OMcp36_27+qd[24]*ROcp36_523;
+    OMcp36_38 = OMcp36_37+qd[24]*ROcp36_623;
+    RLcp36_19 = ROcp36_724*dpt[3][44];
+    RLcp36_29 = ROcp36_824*dpt[3][44];
+    RLcp36_39 = ROcp36_924*dpt[3][44];
+    POcp36_19 = POcp36_17+RLcp36_19;
+    POcp36_29 = POcp36_27+RLcp36_29;
+    POcp36_39 = POcp36_37+RLcp36_39;
+    OMcp36_19 = OMcp36_18+qd[25]*ROcp36_724;
+    OMcp36_29 = OMcp36_28+qd[25]*ROcp36_824;
+    OMcp36_39 = OMcp36_38+qd[25]*ROcp36_924;
+    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
+    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
+    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
+    VIcp36_19 = ORcp36_19+VIcp36_17;
+    VIcp36_29 = ORcp36_29+VIcp36_27;
+    VIcp36_39 = ORcp36_39+VIcp36_37;
+    PxF10[1] = POcp36_19;
+    PxF10[2] = POcp36_29;
+    PxF10[3] = POcp36_39;
+    RxF10[1][1] = ROcp36_125;
+    RxF10[1][2] = ROcp36_225;
+    RxF10[1][3] = ROcp36_325;
+    RxF10[2][1] = ROcp36_425;
+    RxF10[2][2] = ROcp36_525;
+    RxF10[2][3] = ROcp36_625;
+    RxF10[3][1] = ROcp36_724;
+    RxF10[3][2] = ROcp36_824;
+    RxF10[3][3] = ROcp36_924;
+    VxF10[1] = VIcp36_19;
+    VxF10[2] = VIcp36_29;
+    VxF10[3] = VIcp36_39;
+    OMxF10[1] = OMcp36_19;
+    OMxF10[2] = OMcp36_29;
+    OMxF10[3] = OMcp36_39;
+    ROcp37_25 = S4*S5;
+    ROcp37_35 = -C4*S5;
+    ROcp37_85 = -S4*C5;
+    ROcp37_95 = C4*C5;
+    ROcp37_16 = C5*C6;
+    ROcp37_26 = ROcp37_25*C6+C4*S6;
+    ROcp37_36 = ROcp37_35*C6+S4*S6;
+    ROcp37_46 = -C5*S6;
+    ROcp37_56 = -ROcp37_25*S6+C4*C6;
+    ROcp37_66 = -ROcp37_35*S6+S4*C6;
+    ROcp37_423 = ROcp37_46*C23+S23*S5;
+    ROcp37_523 = ROcp37_56*C23+ROcp37_85*S23;
+    ROcp37_623 = ROcp37_66*C23+ROcp37_95*S23;
+    ROcp37_723 = -ROcp37_46*S23+C23*S5;
+    ROcp37_823 = -ROcp37_56*S23+ROcp37_85*C23;
+    ROcp37_923 = -ROcp37_66*S23+ROcp37_95*C23;
+    ROcp37_124 = ROcp37_16*C24-ROcp37_723*S24;
+    ROcp37_224 = ROcp37_26*C24-ROcp37_823*S24;
+    ROcp37_324 = ROcp37_36*C24-ROcp37_923*S24;
+    ROcp37_724 = ROcp37_16*S24+ROcp37_723*C24;
+    ROcp37_824 = ROcp37_26*S24+ROcp37_823*C24;
+    ROcp37_924 = ROcp37_36*S24+ROcp37_923*C24;
+    ROcp37_125 = ROcp37_124*C25+ROcp37_423*S25;
+    ROcp37_225 = ROcp37_224*C25+ROcp37_523*S25;
+    ROcp37_325 = ROcp37_324*C25+ROcp37_623*S25;
+    ROcp37_425 = -ROcp37_124*S25+ROcp37_423*C25;
+    ROcp37_525 = -ROcp37_224*S25+ROcp37_523*C25;
+    ROcp37_625 = -ROcp37_324*S25+ROcp37_623*C25;
+    ROcp37_126 = ROcp37_125*C26-ROcp37_724*S26;
+    ROcp37_226 = ROcp37_225*C26-ROcp37_824*S26;
+    ROcp37_326 = ROcp37_325*C26-ROcp37_924*S26;
+    ROcp37_726 = ROcp37_125*S26+ROcp37_724*C26;
+    ROcp37_826 = ROcp37_225*S26+ROcp37_824*C26;
+    ROcp37_926 = ROcp37_325*S26+ROcp37_924*C26;
+    ROcp37_427 = ROcp37_425*C27+ROcp37_726*S27;
+    ROcp37_527 = ROcp37_525*C27+ROcp37_826*S27;
+    ROcp37_627 = ROcp37_625*C27+ROcp37_926*S27;
+    ROcp37_727 = -ROcp37_425*S27+ROcp37_726*C27;
+    ROcp37_827 = -ROcp37_525*S27+ROcp37_826*C27;
+    ROcp37_927 = -ROcp37_625*S27+ROcp37_926*C27;
+    ROcp37_128 = ROcp37_126*C28+ROcp37_427*S28;
+    ROcp37_228 = ROcp37_226*C28+ROcp37_527*S28;
+    ROcp37_328 = ROcp37_326*C28+ROcp37_627*S28;
+    ROcp37_428 = -ROcp37_126*S28+ROcp37_427*C28;
+    ROcp37_528 = -ROcp37_226*S28+ROcp37_527*C28;
+    ROcp37_628 = -ROcp37_326*S28+ROcp37_627*C28;
+    OMcp37_25 = qd[5]*C4;
+    OMcp37_35 = qd[5]*S4;
+    OMcp37_16 = qd[4]+qd[6]*S5;
+    OMcp37_26 = OMcp37_25+qd[6]*ROcp37_85;
+    OMcp37_36 = OMcp37_35+qd[6]*ROcp37_95;
+    RLcp37_17 = ROcp37_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp37_27 = ROcp37_26*dpt[1][4]+ROcp37_85*dpt[3][4];
+    RLcp37_37 = ROcp37_36*dpt[1][4]+ROcp37_95*dpt[3][4];
+    POcp37_17 = q[1]+RLcp37_17;
+    POcp37_27 = q[2]+RLcp37_27;
+    POcp37_37 = q[3]+RLcp37_37;
+    OMcp37_17 = OMcp37_16+qd[23]*ROcp37_16;
+    OMcp37_27 = OMcp37_26+qd[23]*ROcp37_26;
+    OMcp37_37 = OMcp37_36+qd[23]*ROcp37_36;
+    ORcp37_17 = OMcp37_26*RLcp37_37-OMcp37_36*RLcp37_27;
+    ORcp37_27 = -OMcp37_16*RLcp37_37+OMcp37_36*RLcp37_17;
+    ORcp37_37 = OMcp37_16*RLcp37_27-OMcp37_26*RLcp37_17;
+    VIcp37_17 = qd[1]+ORcp37_17;
+    VIcp37_27 = qd[2]+ORcp37_27;
+    VIcp37_37 = qd[3]+ORcp37_37;
+    OMcp37_18 = OMcp37_17+qd[24]*ROcp37_423;
+    OMcp37_28 = OMcp37_27+qd[24]*ROcp37_523;
+    OMcp37_38 = OMcp37_37+qd[24]*ROcp37_623;
+    RLcp37_19 = ROcp37_724*dpt[3][44];
+    RLcp37_29 = ROcp37_824*dpt[3][44];
+    RLcp37_39 = ROcp37_924*dpt[3][44];
+    POcp37_19 = POcp37_17+RLcp37_19;
+    POcp37_29 = POcp37_27+RLcp37_29;
+    POcp37_39 = POcp37_37+RLcp37_39;
+    OMcp37_19 = OMcp37_18+qd[25]*ROcp37_724;
+    OMcp37_29 = OMcp37_28+qd[25]*ROcp37_824;
+    OMcp37_39 = OMcp37_38+qd[25]*ROcp37_924;
+    ORcp37_19 = OMcp37_28*RLcp37_39-OMcp37_38*RLcp37_29;
+    ORcp37_29 = -OMcp37_18*RLcp37_39+OMcp37_38*RLcp37_19;
+    ORcp37_39 = OMcp37_18*RLcp37_29-OMcp37_28*RLcp37_19;
+    VIcp37_19 = ORcp37_19+VIcp37_17;
+    VIcp37_29 = ORcp37_29+VIcp37_27;
+    VIcp37_39 = ORcp37_39+VIcp37_37;
+    RLcp37_110 = ROcp37_125*dpt[1][48]+ROcp37_425*dpt[2][48]+ROcp37_724*dpt[3][48];
+    RLcp37_210 = ROcp37_225*dpt[1][48]+ROcp37_525*dpt[2][48]+ROcp37_824*dpt[3][48];
+    RLcp37_310 = ROcp37_325*dpt[1][48]+ROcp37_625*dpt[2][48]+ROcp37_924*dpt[3][48];
+    POcp37_110 = POcp37_19+RLcp37_110;
+    POcp37_210 = POcp37_29+RLcp37_210;
+    POcp37_310 = POcp37_39+RLcp37_310;
+    OMcp37_110 = OMcp37_19+qd[26]*ROcp37_425;
+    OMcp37_210 = OMcp37_29+qd[26]*ROcp37_525;
+    OMcp37_310 = OMcp37_39+qd[26]*ROcp37_625;
+    ORcp37_110 = OMcp37_29*RLcp37_310-OMcp37_39*RLcp37_210;
+    ORcp37_210 = -OMcp37_19*RLcp37_310+OMcp37_39*RLcp37_110;
+    ORcp37_310 = OMcp37_19*RLcp37_210-OMcp37_29*RLcp37_110;
+    VIcp37_110 = ORcp37_110+VIcp37_19;
+    VIcp37_210 = ORcp37_210+VIcp37_29;
+    VIcp37_310 = ORcp37_310+VIcp37_39;
+    RLcp37_111 = ROcp37_425*dpt[2][51];
+    RLcp37_211 = ROcp37_525*dpt[2][51];
+    RLcp37_311 = ROcp37_625*dpt[2][51];
+    POcp37_111 = POcp37_110+RLcp37_111;
+    POcp37_211 = POcp37_210+RLcp37_211;
+    POcp37_311 = POcp37_310+RLcp37_311;
+    OMcp37_111 = OMcp37_110+qd[27]*ROcp37_126;
+    OMcp37_211 = OMcp37_210+qd[27]*ROcp37_226;
+    OMcp37_311 = OMcp37_310+qd[27]*ROcp37_326;
+    ORcp37_111 = OMcp37_210*RLcp37_311-OMcp37_310*RLcp37_211;
+    ORcp37_211 = -OMcp37_110*RLcp37_311+OMcp37_310*RLcp37_111;
+    ORcp37_311 = OMcp37_110*RLcp37_211-OMcp37_210*RLcp37_111;
+    VIcp37_111 = ORcp37_111+VIcp37_110;
+    VIcp37_211 = ORcp37_211+VIcp37_210;
+    VIcp37_311 = ORcp37_311+VIcp37_310;
+    RLcp37_112 = ROcp37_727*dpt[3][53];
+    RLcp37_212 = ROcp37_827*dpt[3][53];
+    RLcp37_312 = ROcp37_927*dpt[3][53];
+    POcp37_112 = POcp37_111+RLcp37_112;
+    POcp37_212 = POcp37_211+RLcp37_212;
+    POcp37_312 = POcp37_311+RLcp37_312;
+    OMcp37_112 = OMcp37_111+qd[28]*ROcp37_727;
+    OMcp37_212 = OMcp37_211+qd[28]*ROcp37_827;
+    OMcp37_312 = OMcp37_311+qd[28]*ROcp37_927;
+    ORcp37_112 = OMcp37_211*RLcp37_312-OMcp37_311*RLcp37_212;
+    ORcp37_212 = -OMcp37_111*RLcp37_312+OMcp37_311*RLcp37_112;
+    ORcp37_312 = OMcp37_111*RLcp37_212-OMcp37_211*RLcp37_112;
+    VIcp37_112 = ORcp37_112+VIcp37_111;
+    VIcp37_212 = ORcp37_212+VIcp37_211;
+    VIcp37_312 = ORcp37_312+VIcp37_311;
+    PxF11[1] = POcp37_112;
+    PxF11[2] = POcp37_212;
+    PxF11[3] = POcp37_312;
+    RxF11[1][1] = ROcp37_128;
+    RxF11[1][2] = ROcp37_228;
+    RxF11[1][3] = ROcp37_328;
+    RxF11[2][1] = ROcp37_428;
+    RxF11[2][2] = ROcp37_528;
+    RxF11[2][3] = ROcp37_628;
+    RxF11[3][1] = ROcp37_727;
+    RxF11[3][2] = ROcp37_827;
+    RxF11[3][3] = ROcp37_927;
+    VxF11[1] = VIcp37_112;
+    VxF11[2] = VIcp37_212;
+    VxF11[3] = VIcp37_312;
+    OMxF11[1] = OMcp37_112;
+    OMxF11[2] = OMcp37_212;
+    OMxF11[3] = OMcp37_312;
+    ROcp38_25 = S4*S5;
+    ROcp38_35 = -C4*S5;
+    ROcp38_85 = -S4*C5;
+    ROcp38_95 = C4*C5;
+    ROcp38_16 = C5*C6;
+    ROcp38_26 = ROcp38_25*C6+C4*S6;
+    ROcp38_36 = ROcp38_35*C6+S4*S6;
+    ROcp38_46 = -C5*S6;
+    ROcp38_56 = -ROcp38_25*S6+C4*C6;
+    ROcp38_66 = -ROcp38_35*S6+S4*C6;
+    ROcp38_423 = ROcp38_46*C23+S23*S5;
+    ROcp38_523 = ROcp38_56*C23+ROcp38_85*S23;
+    ROcp38_623 = ROcp38_66*C23+ROcp38_95*S23;
+    ROcp38_723 = -ROcp38_46*S23+C23*S5;
+    ROcp38_823 = -ROcp38_56*S23+ROcp38_85*C23;
+    ROcp38_923 = -ROcp38_66*S23+ROcp38_95*C23;
+    ROcp38_124 = ROcp38_16*C24-ROcp38_723*S24;
+    ROcp38_224 = ROcp38_26*C24-ROcp38_823*S24;
+    ROcp38_324 = ROcp38_36*C24-ROcp38_923*S24;
+    ROcp38_724 = ROcp38_16*S24+ROcp38_723*C24;
+    ROcp38_824 = ROcp38_26*S24+ROcp38_823*C24;
+    ROcp38_924 = ROcp38_36*S24+ROcp38_923*C24;
+    ROcp38_125 = ROcp38_124*C25+ROcp38_423*S25;
+    ROcp38_225 = ROcp38_224*C25+ROcp38_523*S25;
+    ROcp38_325 = ROcp38_324*C25+ROcp38_623*S25;
+    ROcp38_425 = -ROcp38_124*S25+ROcp38_423*C25;
+    ROcp38_525 = -ROcp38_224*S25+ROcp38_523*C25;
+    ROcp38_625 = -ROcp38_324*S25+ROcp38_623*C25;
+    ROcp38_126 = ROcp38_125*C26-ROcp38_724*S26;
+    ROcp38_226 = ROcp38_225*C26-ROcp38_824*S26;
+    ROcp38_326 = ROcp38_325*C26-ROcp38_924*S26;
+    ROcp38_726 = ROcp38_125*S26+ROcp38_724*C26;
+    ROcp38_826 = ROcp38_225*S26+ROcp38_824*C26;
+    ROcp38_926 = ROcp38_325*S26+ROcp38_924*C26;
+    ROcp38_427 = ROcp38_425*C27+ROcp38_726*S27;
+    ROcp38_527 = ROcp38_525*C27+ROcp38_826*S27;
+    ROcp38_627 = ROcp38_625*C27+ROcp38_926*S27;
+    ROcp38_727 = -ROcp38_425*S27+ROcp38_726*C27;
+    ROcp38_827 = -ROcp38_525*S27+ROcp38_826*C27;
+    ROcp38_927 = -ROcp38_625*S27+ROcp38_926*C27;
+    ROcp38_128 = ROcp38_126*C28+ROcp38_427*S28;
+    ROcp38_228 = ROcp38_226*C28+ROcp38_527*S28;
+    ROcp38_328 = ROcp38_326*C28+ROcp38_627*S28;
+    ROcp38_428 = -ROcp38_126*S28+ROcp38_427*C28;
+    ROcp38_528 = -ROcp38_226*S28+ROcp38_527*C28;
+    ROcp38_628 = -ROcp38_326*S28+ROcp38_627*C28;
+    ROcp38_129 = ROcp38_128*C29-ROcp38_727*S29;
+    ROcp38_229 = ROcp38_228*C29-ROcp38_827*S29;
+    ROcp38_329 = ROcp38_328*C29-ROcp38_927*S29;
+    ROcp38_729 = ROcp38_128*S29+ROcp38_727*C29;
+    ROcp38_829 = ROcp38_228*S29+ROcp38_827*C29;
+    ROcp38_929 = ROcp38_328*S29+ROcp38_927*C29;
+    OMcp38_25 = qd[5]*C4;
+    OMcp38_35 = qd[5]*S4;
+    OMcp38_16 = qd[4]+qd[6]*S5;
+    OMcp38_26 = OMcp38_25+qd[6]*ROcp38_85;
+    OMcp38_36 = OMcp38_35+qd[6]*ROcp38_95;
+    RLcp38_17 = ROcp38_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp38_27 = ROcp38_26*dpt[1][4]+ROcp38_85*dpt[3][4];
+    RLcp38_37 = ROcp38_36*dpt[1][4]+ROcp38_95*dpt[3][4];
+    POcp38_17 = q[1]+RLcp38_17;
+    POcp38_27 = q[2]+RLcp38_27;
+    POcp38_37 = q[3]+RLcp38_37;
+    OMcp38_17 = OMcp38_16+qd[23]*ROcp38_16;
+    OMcp38_27 = OMcp38_26+qd[23]*ROcp38_26;
+    OMcp38_37 = OMcp38_36+qd[23]*ROcp38_36;
+    ORcp38_17 = OMcp38_26*RLcp38_37-OMcp38_36*RLcp38_27;
+    ORcp38_27 = -OMcp38_16*RLcp38_37+OMcp38_36*RLcp38_17;
+    ORcp38_37 = OMcp38_16*RLcp38_27-OMcp38_26*RLcp38_17;
+    VIcp38_17 = qd[1]+ORcp38_17;
+    VIcp38_27 = qd[2]+ORcp38_27;
+    VIcp38_37 = qd[3]+ORcp38_37;
+    OMcp38_18 = OMcp38_17+qd[24]*ROcp38_423;
+    OMcp38_28 = OMcp38_27+qd[24]*ROcp38_523;
+    OMcp38_38 = OMcp38_37+qd[24]*ROcp38_623;
+    RLcp38_19 = ROcp38_724*dpt[3][44];
+    RLcp38_29 = ROcp38_824*dpt[3][44];
+    RLcp38_39 = ROcp38_924*dpt[3][44];
+    POcp38_19 = POcp38_17+RLcp38_19;
+    POcp38_29 = POcp38_27+RLcp38_29;
+    POcp38_39 = POcp38_37+RLcp38_39;
+    OMcp38_19 = OMcp38_18+qd[25]*ROcp38_724;
+    OMcp38_29 = OMcp38_28+qd[25]*ROcp38_824;
+    OMcp38_39 = OMcp38_38+qd[25]*ROcp38_924;
+    ORcp38_19 = OMcp38_28*RLcp38_39-OMcp38_38*RLcp38_29;
+    ORcp38_29 = -OMcp38_18*RLcp38_39+OMcp38_38*RLcp38_19;
+    ORcp38_39 = OMcp38_18*RLcp38_29-OMcp38_28*RLcp38_19;
+    VIcp38_19 = ORcp38_19+VIcp38_17;
+    VIcp38_29 = ORcp38_29+VIcp38_27;
+    VIcp38_39 = ORcp38_39+VIcp38_37;
+    RLcp38_110 = ROcp38_125*dpt[1][48]+ROcp38_425*dpt[2][48]+ROcp38_724*dpt[3][48];
+    RLcp38_210 = ROcp38_225*dpt[1][48]+ROcp38_525*dpt[2][48]+ROcp38_824*dpt[3][48];
+    RLcp38_310 = ROcp38_325*dpt[1][48]+ROcp38_625*dpt[2][48]+ROcp38_924*dpt[3][48];
+    POcp38_110 = POcp38_19+RLcp38_110;
+    POcp38_210 = POcp38_29+RLcp38_210;
+    POcp38_310 = POcp38_39+RLcp38_310;
+    OMcp38_110 = OMcp38_19+qd[26]*ROcp38_425;
+    OMcp38_210 = OMcp38_29+qd[26]*ROcp38_525;
+    OMcp38_310 = OMcp38_39+qd[26]*ROcp38_625;
+    ORcp38_110 = OMcp38_29*RLcp38_310-OMcp38_39*RLcp38_210;
+    ORcp38_210 = -OMcp38_19*RLcp38_310+OMcp38_39*RLcp38_110;
+    ORcp38_310 = OMcp38_19*RLcp38_210-OMcp38_29*RLcp38_110;
+    VIcp38_110 = ORcp38_110+VIcp38_19;
+    VIcp38_210 = ORcp38_210+VIcp38_29;
+    VIcp38_310 = ORcp38_310+VIcp38_39;
+    RLcp38_111 = ROcp38_425*dpt[2][51];
+    RLcp38_211 = ROcp38_525*dpt[2][51];
+    RLcp38_311 = ROcp38_625*dpt[2][51];
+    POcp38_111 = POcp38_110+RLcp38_111;
+    POcp38_211 = POcp38_210+RLcp38_211;
+    POcp38_311 = POcp38_310+RLcp38_311;
+    OMcp38_111 = OMcp38_110+qd[27]*ROcp38_126;
+    OMcp38_211 = OMcp38_210+qd[27]*ROcp38_226;
+    OMcp38_311 = OMcp38_310+qd[27]*ROcp38_326;
+    ORcp38_111 = OMcp38_210*RLcp38_311-OMcp38_310*RLcp38_211;
+    ORcp38_211 = -OMcp38_110*RLcp38_311+OMcp38_310*RLcp38_111;
+    ORcp38_311 = OMcp38_110*RLcp38_211-OMcp38_210*RLcp38_111;
+    VIcp38_111 = ORcp38_111+VIcp38_110;
+    VIcp38_211 = ORcp38_211+VIcp38_210;
+    VIcp38_311 = ORcp38_311+VIcp38_310;
+    RLcp38_112 = ROcp38_727*dpt[3][53];
+    RLcp38_212 = ROcp38_827*dpt[3][53];
+    RLcp38_312 = ROcp38_927*dpt[3][53];
+    POcp38_112 = POcp38_111+RLcp38_112;
+    POcp38_212 = POcp38_211+RLcp38_212;
+    POcp38_312 = POcp38_311+RLcp38_312;
+    OMcp38_112 = OMcp38_111+qd[28]*ROcp38_727;
+    OMcp38_212 = OMcp38_211+qd[28]*ROcp38_827;
+    OMcp38_312 = OMcp38_311+qd[28]*ROcp38_927;
+    ORcp38_112 = OMcp38_211*RLcp38_312-OMcp38_311*RLcp38_212;
+    ORcp38_212 = -OMcp38_111*RLcp38_312+OMcp38_311*RLcp38_112;
+    ORcp38_312 = OMcp38_111*RLcp38_212-OMcp38_211*RLcp38_112;
+    VIcp38_112 = ORcp38_112+VIcp38_111;
+    VIcp38_212 = ORcp38_212+VIcp38_211;
+    VIcp38_312 = ORcp38_312+VIcp38_311;
+    RLcp38_113 = ROcp38_727*dpt[3][56];
+    RLcp38_213 = ROcp38_827*dpt[3][56];
+    RLcp38_313 = ROcp38_927*dpt[3][56];
+    POcp38_113 = POcp38_112+RLcp38_113;
+    POcp38_213 = POcp38_212+RLcp38_213;
+    POcp38_313 = POcp38_312+RLcp38_313;
+    OMcp38_113 = OMcp38_112+qd[29]*ROcp38_428;
+    OMcp38_213 = OMcp38_212+qd[29]*ROcp38_528;
+    OMcp38_313 = OMcp38_312+qd[29]*ROcp38_628;
+    ORcp38_113 = OMcp38_212*RLcp38_313-OMcp38_312*RLcp38_213;
+    ORcp38_213 = -OMcp38_112*RLcp38_313+OMcp38_312*RLcp38_113;
+    ORcp38_313 = OMcp38_112*RLcp38_213-OMcp38_212*RLcp38_113;
+    VIcp38_113 = ORcp38_113+VIcp38_112;
+    VIcp38_213 = ORcp38_213+VIcp38_212;
+    VIcp38_313 = ORcp38_313+VIcp38_312;
+    PxF12[1] = POcp38_113;
+    PxF12[2] = POcp38_213;
+    PxF12[3] = POcp38_313;
+    RxF12[1][1] = ROcp38_129;
+    RxF12[1][2] = ROcp38_229;
+    RxF12[1][3] = ROcp38_329;
+    RxF12[2][1] = ROcp38_428;
+    RxF12[2][2] = ROcp38_528;
+    RxF12[2][3] = ROcp38_628;
+    RxF12[3][1] = ROcp38_729;
+    RxF12[3][2] = ROcp38_829;
+    RxF12[3][3] = ROcp38_929;
+    VxF12[1] = VIcp38_113;
+    VxF12[2] = VIcp38_213;
+    VxF12[3] = VIcp38_313;
+    OMxF12[1] = OMcp38_113;
+    OMxF12[2] = OMcp38_213;
+    OMxF12[3] = OMcp38_313;
+    ROcp39_25 = S4*S5;
+    ROcp39_35 = -C4*S5;
+    ROcp39_85 = -S4*C5;
+    ROcp39_95 = C4*C5;
+    ROcp39_16 = C5*C6;
+    ROcp39_26 = ROcp39_25*C6+C4*S6;
+    ROcp39_36 = ROcp39_35*C6+S4*S6;
+    ROcp39_46 = -C5*S6;
+    ROcp39_56 = -ROcp39_25*S6+C4*C6;
+    ROcp39_66 = -ROcp39_35*S6+S4*C6;
+    ROcp39_423 = ROcp39_46*C23+S23*S5;
+    ROcp39_523 = ROcp39_56*C23+ROcp39_85*S23;
+    ROcp39_623 = ROcp39_66*C23+ROcp39_95*S23;
+    ROcp39_723 = -ROcp39_46*S23+C23*S5;
+    ROcp39_823 = -ROcp39_56*S23+ROcp39_85*C23;
+    ROcp39_923 = -ROcp39_66*S23+ROcp39_95*C23;
+    ROcp39_124 = ROcp39_16*C24-ROcp39_723*S24;
+    ROcp39_224 = ROcp39_26*C24-ROcp39_823*S24;
+    ROcp39_324 = ROcp39_36*C24-ROcp39_923*S24;
+    ROcp39_724 = ROcp39_16*S24+ROcp39_723*C24;
+    ROcp39_824 = ROcp39_26*S24+ROcp39_823*C24;
+    ROcp39_924 = ROcp39_36*S24+ROcp39_923*C24;
+    ROcp39_125 = ROcp39_124*C25+ROcp39_423*S25;
+    ROcp39_225 = ROcp39_224*C25+ROcp39_523*S25;
+    ROcp39_325 = ROcp39_324*C25+ROcp39_623*S25;
+    ROcp39_425 = -ROcp39_124*S25+ROcp39_423*C25;
+    ROcp39_525 = -ROcp39_224*S25+ROcp39_523*C25;
+    ROcp39_625 = -ROcp39_324*S25+ROcp39_623*C25;
+    ROcp39_130 = ROcp39_125*C30-ROcp39_724*S30;
+    ROcp39_230 = ROcp39_225*C30-ROcp39_824*S30;
+    ROcp39_330 = ROcp39_325*C30-ROcp39_924*S30;
+    ROcp39_730 = ROcp39_125*S30+ROcp39_724*C30;
+    ROcp39_830 = ROcp39_225*S30+ROcp39_824*C30;
+    ROcp39_930 = ROcp39_325*S30+ROcp39_924*C30;
+    ROcp39_431 = ROcp39_425*C31+ROcp39_730*S31;
+    ROcp39_531 = ROcp39_525*C31+ROcp39_830*S31;
+    ROcp39_631 = ROcp39_625*C31+ROcp39_930*S31;
+    ROcp39_731 = -ROcp39_425*S31+ROcp39_730*C31;
+    ROcp39_831 = -ROcp39_525*S31+ROcp39_830*C31;
+    ROcp39_931 = -ROcp39_625*S31+ROcp39_930*C31;
+    ROcp39_132 = ROcp39_130*C32+ROcp39_431*S32;
+    ROcp39_232 = ROcp39_230*C32+ROcp39_531*S32;
+    ROcp39_332 = ROcp39_330*C32+ROcp39_631*S32;
+    ROcp39_432 = -ROcp39_130*S32+ROcp39_431*C32;
+    ROcp39_532 = -ROcp39_230*S32+ROcp39_531*C32;
+    ROcp39_632 = -ROcp39_330*S32+ROcp39_631*C32;
+    OMcp39_25 = qd[5]*C4;
+    OMcp39_35 = qd[5]*S4;
+    OMcp39_16 = qd[4]+qd[6]*S5;
+    OMcp39_26 = OMcp39_25+qd[6]*ROcp39_85;
+    OMcp39_36 = OMcp39_35+qd[6]*ROcp39_95;
+    RLcp39_17 = ROcp39_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp39_27 = ROcp39_26*dpt[1][4]+ROcp39_85*dpt[3][4];
+    RLcp39_37 = ROcp39_36*dpt[1][4]+ROcp39_95*dpt[3][4];
+    POcp39_17 = q[1]+RLcp39_17;
+    POcp39_27 = q[2]+RLcp39_27;
+    POcp39_37 = q[3]+RLcp39_37;
+    OMcp39_17 = OMcp39_16+qd[23]*ROcp39_16;
+    OMcp39_27 = OMcp39_26+qd[23]*ROcp39_26;
+    OMcp39_37 = OMcp39_36+qd[23]*ROcp39_36;
+    ORcp39_17 = OMcp39_26*RLcp39_37-OMcp39_36*RLcp39_27;
+    ORcp39_27 = -OMcp39_16*RLcp39_37+OMcp39_36*RLcp39_17;
+    ORcp39_37 = OMcp39_16*RLcp39_27-OMcp39_26*RLcp39_17;
+    VIcp39_17 = qd[1]+ORcp39_17;
+    VIcp39_27 = qd[2]+ORcp39_27;
+    VIcp39_37 = qd[3]+ORcp39_37;
+    OMcp39_18 = OMcp39_17+qd[24]*ROcp39_423;
+    OMcp39_28 = OMcp39_27+qd[24]*ROcp39_523;
+    OMcp39_38 = OMcp39_37+qd[24]*ROcp39_623;
+    RLcp39_19 = ROcp39_724*dpt[3][44];
+    RLcp39_29 = ROcp39_824*dpt[3][44];
+    RLcp39_39 = ROcp39_924*dpt[3][44];
+    POcp39_19 = POcp39_17+RLcp39_19;
+    POcp39_29 = POcp39_27+RLcp39_29;
+    POcp39_39 = POcp39_37+RLcp39_39;
+    OMcp39_19 = OMcp39_18+qd[25]*ROcp39_724;
+    OMcp39_29 = OMcp39_28+qd[25]*ROcp39_824;
+    OMcp39_39 = OMcp39_38+qd[25]*ROcp39_924;
+    ORcp39_19 = OMcp39_28*RLcp39_39-OMcp39_38*RLcp39_29;
+    ORcp39_29 = -OMcp39_18*RLcp39_39+OMcp39_38*RLcp39_19;
+    ORcp39_39 = OMcp39_18*RLcp39_29-OMcp39_28*RLcp39_19;
+    VIcp39_19 = ORcp39_19+VIcp39_17;
+    VIcp39_29 = ORcp39_29+VIcp39_27;
+    VIcp39_39 = ORcp39_39+VIcp39_37;
+    RLcp39_110 = ROcp39_125*dpt[1][49]+ROcp39_425*dpt[2][49]+ROcp39_724*dpt[3][49];
+    RLcp39_210 = ROcp39_225*dpt[1][49]+ROcp39_525*dpt[2][49]+ROcp39_824*dpt[3][49];
+    RLcp39_310 = ROcp39_325*dpt[1][49]+ROcp39_625*dpt[2][49]+ROcp39_924*dpt[3][49];
+    POcp39_110 = POcp39_19+RLcp39_110;
+    POcp39_210 = POcp39_29+RLcp39_210;
+    POcp39_310 = POcp39_39+RLcp39_310;
+    OMcp39_110 = OMcp39_19+qd[30]*ROcp39_425;
+    OMcp39_210 = OMcp39_29+qd[30]*ROcp39_525;
+    OMcp39_310 = OMcp39_39+qd[30]*ROcp39_625;
+    ORcp39_110 = OMcp39_29*RLcp39_310-OMcp39_39*RLcp39_210;
+    ORcp39_210 = -OMcp39_19*RLcp39_310+OMcp39_39*RLcp39_110;
+    ORcp39_310 = OMcp39_19*RLcp39_210-OMcp39_29*RLcp39_110;
+    VIcp39_110 = ORcp39_110+VIcp39_19;
+    VIcp39_210 = ORcp39_210+VIcp39_29;
+    VIcp39_310 = ORcp39_310+VIcp39_39;
+    RLcp39_111 = ROcp39_425*dpt[2][60];
+    RLcp39_211 = ROcp39_525*dpt[2][60];
+    RLcp39_311 = ROcp39_625*dpt[2][60];
+    POcp39_111 = POcp39_110+RLcp39_111;
+    POcp39_211 = POcp39_210+RLcp39_211;
+    POcp39_311 = POcp39_310+RLcp39_311;
+    OMcp39_111 = OMcp39_110+qd[31]*ROcp39_130;
+    OMcp39_211 = OMcp39_210+qd[31]*ROcp39_230;
+    OMcp39_311 = OMcp39_310+qd[31]*ROcp39_330;
+    ORcp39_111 = OMcp39_210*RLcp39_311-OMcp39_310*RLcp39_211;
+    ORcp39_211 = -OMcp39_110*RLcp39_311+OMcp39_310*RLcp39_111;
+    ORcp39_311 = OMcp39_110*RLcp39_211-OMcp39_210*RLcp39_111;
+    VIcp39_111 = ORcp39_111+VIcp39_110;
+    VIcp39_211 = ORcp39_211+VIcp39_210;
+    VIcp39_311 = ORcp39_311+VIcp39_310;
+    RLcp39_112 = ROcp39_731*dpt[3][62];
+    RLcp39_212 = ROcp39_831*dpt[3][62];
+    RLcp39_312 = ROcp39_931*dpt[3][62];
+    POcp39_112 = POcp39_111+RLcp39_112;
+    POcp39_212 = POcp39_211+RLcp39_212;
+    POcp39_312 = POcp39_311+RLcp39_312;
+    OMcp39_112 = OMcp39_111+qd[32]*ROcp39_731;
+    OMcp39_212 = OMcp39_211+qd[32]*ROcp39_831;
+    OMcp39_312 = OMcp39_311+qd[32]*ROcp39_931;
+    ORcp39_112 = OMcp39_211*RLcp39_312-OMcp39_311*RLcp39_212;
+    ORcp39_212 = -OMcp39_111*RLcp39_312+OMcp39_311*RLcp39_112;
+    ORcp39_312 = OMcp39_111*RLcp39_212-OMcp39_211*RLcp39_112;
+    VIcp39_112 = ORcp39_112+VIcp39_111;
+    VIcp39_212 = ORcp39_212+VIcp39_211;
+    VIcp39_312 = ORcp39_312+VIcp39_311;
+    PxF13[1] = POcp39_112;
+    PxF13[2] = POcp39_212;
+    PxF13[3] = POcp39_312;
+    RxF13[1][1] = ROcp39_132;
+    RxF13[1][2] = ROcp39_232;
+    RxF13[1][3] = ROcp39_332;
+    RxF13[2][1] = ROcp39_432;
+    RxF13[2][2] = ROcp39_532;
+    RxF13[2][3] = ROcp39_632;
+    RxF13[3][1] = ROcp39_731;
+    RxF13[3][2] = ROcp39_831;
+    RxF13[3][3] = ROcp39_931;
+    VxF13[1] = VIcp39_112;
+    VxF13[2] = VIcp39_212;
+    VxF13[3] = VIcp39_312;
+    OMxF13[1] = OMcp39_112;
+    OMxF13[2] = OMcp39_212;
+    OMxF13[3] = OMcp39_312;
+    ROcp40_25 = S4*S5;
+    ROcp40_35 = -C4*S5;
+    ROcp40_85 = -S4*C5;
+    ROcp40_95 = C4*C5;
+    ROcp40_16 = C5*C6;
+    ROcp40_26 = ROcp40_25*C6+C4*S6;
+    ROcp40_36 = ROcp40_35*C6+S4*S6;
+    ROcp40_46 = -C5*S6;
+    ROcp40_56 = -ROcp40_25*S6+C4*C6;
+    ROcp40_66 = -ROcp40_35*S6+S4*C6;
+    ROcp40_423 = ROcp40_46*C23+S23*S5;
+    ROcp40_523 = ROcp40_56*C23+ROcp40_85*S23;
+    ROcp40_623 = ROcp40_66*C23+ROcp40_95*S23;
+    ROcp40_723 = -ROcp40_46*S23+C23*S5;
+    ROcp40_823 = -ROcp40_56*S23+ROcp40_85*C23;
+    ROcp40_923 = -ROcp40_66*S23+ROcp40_95*C23;
+    ROcp40_124 = ROcp40_16*C24-ROcp40_723*S24;
+    ROcp40_224 = ROcp40_26*C24-ROcp40_823*S24;
+    ROcp40_324 = ROcp40_36*C24-ROcp40_923*S24;
+    ROcp40_724 = ROcp40_16*S24+ROcp40_723*C24;
+    ROcp40_824 = ROcp40_26*S24+ROcp40_823*C24;
+    ROcp40_924 = ROcp40_36*S24+ROcp40_923*C24;
+    ROcp40_125 = ROcp40_124*C25+ROcp40_423*S25;
+    ROcp40_225 = ROcp40_224*C25+ROcp40_523*S25;
+    ROcp40_325 = ROcp40_324*C25+ROcp40_623*S25;
+    ROcp40_425 = -ROcp40_124*S25+ROcp40_423*C25;
+    ROcp40_525 = -ROcp40_224*S25+ROcp40_523*C25;
+    ROcp40_625 = -ROcp40_324*S25+ROcp40_623*C25;
+    ROcp40_130 = ROcp40_125*C30-ROcp40_724*S30;
+    ROcp40_230 = ROcp40_225*C30-ROcp40_824*S30;
+    ROcp40_330 = ROcp40_325*C30-ROcp40_924*S30;
+    ROcp40_730 = ROcp40_125*S30+ROcp40_724*C30;
+    ROcp40_830 = ROcp40_225*S30+ROcp40_824*C30;
+    ROcp40_930 = ROcp40_325*S30+ROcp40_924*C30;
+    ROcp40_431 = ROcp40_425*C31+ROcp40_730*S31;
+    ROcp40_531 = ROcp40_525*C31+ROcp40_830*S31;
+    ROcp40_631 = ROcp40_625*C31+ROcp40_930*S31;
+    ROcp40_731 = -ROcp40_425*S31+ROcp40_730*C31;
+    ROcp40_831 = -ROcp40_525*S31+ROcp40_830*C31;
+    ROcp40_931 = -ROcp40_625*S31+ROcp40_930*C31;
+    ROcp40_132 = ROcp40_130*C32+ROcp40_431*S32;
+    ROcp40_232 = ROcp40_230*C32+ROcp40_531*S32;
+    ROcp40_332 = ROcp40_330*C32+ROcp40_631*S32;
+    ROcp40_432 = -ROcp40_130*S32+ROcp40_431*C32;
+    ROcp40_532 = -ROcp40_230*S32+ROcp40_531*C32;
+    ROcp40_632 = -ROcp40_330*S32+ROcp40_631*C32;
+    ROcp40_133 = ROcp40_132*C33-ROcp40_731*S33;
+    ROcp40_233 = ROcp40_232*C33-ROcp40_831*S33;
+    ROcp40_333 = ROcp40_332*C33-ROcp40_931*S33;
+    ROcp40_733 = ROcp40_132*S33+ROcp40_731*C33;
+    ROcp40_833 = ROcp40_232*S33+ROcp40_831*C33;
+    ROcp40_933 = ROcp40_332*S33+ROcp40_931*C33;
+    OMcp40_25 = qd[5]*C4;
+    OMcp40_35 = qd[5]*S4;
+    OMcp40_16 = qd[4]+qd[6]*S5;
+    OMcp40_26 = OMcp40_25+qd[6]*ROcp40_85;
+    OMcp40_36 = OMcp40_35+qd[6]*ROcp40_95;
+    RLcp40_17 = ROcp40_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp40_27 = ROcp40_26*dpt[1][4]+ROcp40_85*dpt[3][4];
+    RLcp40_37 = ROcp40_36*dpt[1][4]+ROcp40_95*dpt[3][4];
+    POcp40_17 = q[1]+RLcp40_17;
+    POcp40_27 = q[2]+RLcp40_27;
+    POcp40_37 = q[3]+RLcp40_37;
+    OMcp40_17 = OMcp40_16+qd[23]*ROcp40_16;
+    OMcp40_27 = OMcp40_26+qd[23]*ROcp40_26;
+    OMcp40_37 = OMcp40_36+qd[23]*ROcp40_36;
+    ORcp40_17 = OMcp40_26*RLcp40_37-OMcp40_36*RLcp40_27;
+    ORcp40_27 = -OMcp40_16*RLcp40_37+OMcp40_36*RLcp40_17;
+    ORcp40_37 = OMcp40_16*RLcp40_27-OMcp40_26*RLcp40_17;
+    VIcp40_17 = qd[1]+ORcp40_17;
+    VIcp40_27 = qd[2]+ORcp40_27;
+    VIcp40_37 = qd[3]+ORcp40_37;
+    OMcp40_18 = OMcp40_17+qd[24]*ROcp40_423;
+    OMcp40_28 = OMcp40_27+qd[24]*ROcp40_523;
+    OMcp40_38 = OMcp40_37+qd[24]*ROcp40_623;
+    RLcp40_19 = ROcp40_724*dpt[3][44];
+    RLcp40_29 = ROcp40_824*dpt[3][44];
+    RLcp40_39 = ROcp40_924*dpt[3][44];
+    POcp40_19 = POcp40_17+RLcp40_19;
+    POcp40_29 = POcp40_27+RLcp40_29;
+    POcp40_39 = POcp40_37+RLcp40_39;
+    OMcp40_19 = OMcp40_18+qd[25]*ROcp40_724;
+    OMcp40_29 = OMcp40_28+qd[25]*ROcp40_824;
+    OMcp40_39 = OMcp40_38+qd[25]*ROcp40_924;
+    ORcp40_19 = OMcp40_28*RLcp40_39-OMcp40_38*RLcp40_29;
+    ORcp40_29 = -OMcp40_18*RLcp40_39+OMcp40_38*RLcp40_19;
+    ORcp40_39 = OMcp40_18*RLcp40_29-OMcp40_28*RLcp40_19;
+    VIcp40_19 = ORcp40_19+VIcp40_17;
+    VIcp40_29 = ORcp40_29+VIcp40_27;
+    VIcp40_39 = ORcp40_39+VIcp40_37;
+    RLcp40_110 = ROcp40_125*dpt[1][49]+ROcp40_425*dpt[2][49]+ROcp40_724*dpt[3][49];
+    RLcp40_210 = ROcp40_225*dpt[1][49]+ROcp40_525*dpt[2][49]+ROcp40_824*dpt[3][49];
+    RLcp40_310 = ROcp40_325*dpt[1][49]+ROcp40_625*dpt[2][49]+ROcp40_924*dpt[3][49];
+    POcp40_110 = POcp40_19+RLcp40_110;
+    POcp40_210 = POcp40_29+RLcp40_210;
+    POcp40_310 = POcp40_39+RLcp40_310;
+    OMcp40_110 = OMcp40_19+qd[30]*ROcp40_425;
+    OMcp40_210 = OMcp40_29+qd[30]*ROcp40_525;
+    OMcp40_310 = OMcp40_39+qd[30]*ROcp40_625;
+    ORcp40_110 = OMcp40_29*RLcp40_310-OMcp40_39*RLcp40_210;
+    ORcp40_210 = -OMcp40_19*RLcp40_310+OMcp40_39*RLcp40_110;
+    ORcp40_310 = OMcp40_19*RLcp40_210-OMcp40_29*RLcp40_110;
+    VIcp40_110 = ORcp40_110+VIcp40_19;
+    VIcp40_210 = ORcp40_210+VIcp40_29;
+    VIcp40_310 = ORcp40_310+VIcp40_39;
+    RLcp40_111 = ROcp40_425*dpt[2][60];
+    RLcp40_211 = ROcp40_525*dpt[2][60];
+    RLcp40_311 = ROcp40_625*dpt[2][60];
+    POcp40_111 = POcp40_110+RLcp40_111;
+    POcp40_211 = POcp40_210+RLcp40_211;
+    POcp40_311 = POcp40_310+RLcp40_311;
+    OMcp40_111 = OMcp40_110+qd[31]*ROcp40_130;
+    OMcp40_211 = OMcp40_210+qd[31]*ROcp40_230;
+    OMcp40_311 = OMcp40_310+qd[31]*ROcp40_330;
+    ORcp40_111 = OMcp40_210*RLcp40_311-OMcp40_310*RLcp40_211;
+    ORcp40_211 = -OMcp40_110*RLcp40_311+OMcp40_310*RLcp40_111;
+    ORcp40_311 = OMcp40_110*RLcp40_211-OMcp40_210*RLcp40_111;
+    VIcp40_111 = ORcp40_111+VIcp40_110;
+    VIcp40_211 = ORcp40_211+VIcp40_210;
+    VIcp40_311 = ORcp40_311+VIcp40_310;
+    RLcp40_112 = ROcp40_731*dpt[3][62];
+    RLcp40_212 = ROcp40_831*dpt[3][62];
+    RLcp40_312 = ROcp40_931*dpt[3][62];
+    POcp40_112 = POcp40_111+RLcp40_112;
+    POcp40_212 = POcp40_211+RLcp40_212;
+    POcp40_312 = POcp40_311+RLcp40_312;
+    OMcp40_112 = OMcp40_111+qd[32]*ROcp40_731;
+    OMcp40_212 = OMcp40_211+qd[32]*ROcp40_831;
+    OMcp40_312 = OMcp40_311+qd[32]*ROcp40_931;
+    ORcp40_112 = OMcp40_211*RLcp40_312-OMcp40_311*RLcp40_212;
+    ORcp40_212 = -OMcp40_111*RLcp40_312+OMcp40_311*RLcp40_112;
+    ORcp40_312 = OMcp40_111*RLcp40_212-OMcp40_211*RLcp40_112;
+    VIcp40_112 = ORcp40_112+VIcp40_111;
+    VIcp40_212 = ORcp40_212+VIcp40_211;
+    VIcp40_312 = ORcp40_312+VIcp40_311;
+    RLcp40_113 = ROcp40_731*dpt[3][65];
+    RLcp40_213 = ROcp40_831*dpt[3][65];
+    RLcp40_313 = ROcp40_931*dpt[3][65];
+    POcp40_113 = POcp40_112+RLcp40_113;
+    POcp40_213 = POcp40_212+RLcp40_213;
+    POcp40_313 = POcp40_312+RLcp40_313;
+    OMcp40_113 = OMcp40_112+qd[33]*ROcp40_432;
+    OMcp40_213 = OMcp40_212+qd[33]*ROcp40_532;
+    OMcp40_313 = OMcp40_312+qd[33]*ROcp40_632;
+    ORcp40_113 = OMcp40_212*RLcp40_313-OMcp40_312*RLcp40_213;
+    ORcp40_213 = -OMcp40_112*RLcp40_313+OMcp40_312*RLcp40_113;
+    ORcp40_313 = OMcp40_112*RLcp40_213-OMcp40_212*RLcp40_113;
+    VIcp40_113 = ORcp40_113+VIcp40_112;
+    VIcp40_213 = ORcp40_213+VIcp40_212;
+    VIcp40_313 = ORcp40_313+VIcp40_312;
+    PxF14[1] = POcp40_113;
+    PxF14[2] = POcp40_213;
+    PxF14[3] = POcp40_313;
+    RxF14[1][1] = ROcp40_133;
+    RxF14[1][2] = ROcp40_233;
+    RxF14[1][3] = ROcp40_333;
+    RxF14[2][1] = ROcp40_432;
+    RxF14[2][2] = ROcp40_532;
+    RxF14[2][3] = ROcp40_632;
+    RxF14[3][1] = ROcp40_733;
+    RxF14[3][2] = ROcp40_833;
+    RxF14[3][3] = ROcp40_933;
+    VxF14[1] = VIcp40_113;
+    VxF14[2] = VIcp40_213;
+    VxF14[3] = VIcp40_313;
+    OMxF14[1] = OMcp40_113;
+    OMxF14[2] = OMcp40_213;
+    OMxF14[3] = OMcp40_313;
+    // output copy
+    for(i=1; i<=3; i++)
+    {
+        PxF_tab[0][i] = PxF1[i];
+        PxF_tab[1][i] = PxF2[i];
+        PxF_tab[2][i] = PxF3[i];
+        PxF_tab[3][i] = PxF4[i];
+        PxF_tab[4][i] = PxF5[i];
+        PxF_tab[5][i] = PxF6[i];
+        PxF_tab[6][i] = PxF7[i];
+        PxF_tab[7][i] = PxF8[i];
+        PxF_tab[8][i] = PxF9[i];
+        PxF_tab[9][i] = PxF10[i];
+        PxF_tab[10][i] = PxF11[i];
+        PxF_tab[11][i] = PxF12[i];
+        PxF_tab[12][i] = PxF13[i];
+        PxF_tab[13][i] = PxF14[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        for(j=1; j<=3; j++)
+        {
+        RxF_tab[0][i][j] = RxF1[i][j];
+        RxF_tab[1][i][j] = RxF2[i][j];
+        RxF_tab[2][i][j] = RxF3[i][j];
+        RxF_tab[3][i][j] = RxF4[i][j];
+        RxF_tab[4][i][j] = RxF5[i][j];
+        RxF_tab[5][i][j] = RxF6[i][j];
+        RxF_tab[6][i][j] = RxF7[i][j];
+        RxF_tab[7][i][j] = RxF8[i][j];
+        RxF_tab[8][i][j] = RxF9[i][j];
+        RxF_tab[9][i][j] = RxF10[i][j];
+        RxF_tab[10][i][j] = RxF11[i][j];
+        RxF_tab[11][i][j] = RxF12[i][j];
+        RxF_tab[12][i][j] = RxF13[i][j];
+        RxF_tab[13][i][j] = RxF14[i][j];
+        }
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        VxF_tab[0][i] = VxF1[i];
+        VxF_tab[1][i] = VxF2[i];
+        VxF_tab[2][i] = VxF3[i];
+        VxF_tab[3][i] = VxF4[i];
+        VxF_tab[4][i] = VxF5[i];
+        VxF_tab[5][i] = VxF6[i];
+        VxF_tab[6][i] = VxF7[i];
+        VxF_tab[7][i] = VxF8[i];
+        VxF_tab[8][i] = VxF9[i];
+        VxF_tab[9][i] = VxF10[i];
+        VxF_tab[10][i] = VxF11[i];
+        VxF_tab[11][i] = VxF12[i];
+        VxF_tab[12][i] = VxF13[i];
+        VxF_tab[13][i] = VxF14[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        OMxF_tab[0][i] = OMxF1[i];
+        OMxF_tab[1][i] = OMxF2[i];
+        OMxF_tab[2][i] = OMxF3[i];
+        OMxF_tab[3][i] = OMxF4[i];
+        OMxF_tab[4][i] = OMxF5[i];
+        OMxF_tab[5][i] = OMxF6[i];
+        OMxF_tab[6][i] = OMxF7[i];
+        OMxF_tab[7][i] = OMxF8[i];
+        OMxF_tab[8][i] = OMxF9[i];
+        OMxF_tab[9][i] = OMxF10[i];
+        OMxF_tab[10][i] = OMxF11[i];
+        OMxF_tab[11][i] = OMxF12[i];
+        OMxF_tab[12][i] = OMxF13[i];
+        OMxF_tab[13][i] = OMxF14[i];
+    }
+
+}
diff --git a/dataR/extra/init_feet/specific_simu_index.cc b/dataR/extra/init_feet/specific_simu_index.cc
new file mode 100644
index 0000000..40fc295
--- /dev/null
+++ b/dataR/extra/init_feet/specific_simu_index.cc
@@ -0,0 +1,234 @@
+#include "ModelSimuIndex.hh"
+#include "SimuIndex.hh"
+#include "user_all_id.h"
+
+#include <iostream>
+#include <stdlib.h>
+
+/*! \brief get the indexes related to the MBS file for the joints
+ * 
+ * \param[in] joint_id ID of the requested joint
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsJointId(int joint_id)
+{
+	switch (joint_id)
+	{
+		// floating base
+		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
+		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
+		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
+		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
+		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
+		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
+
+		// right leg
+		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
+		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
+		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
+		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
+		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
+		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
+
+		// left leg
+		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
+		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
+		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
+		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
+		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
+		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
+
+		// torso
+		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
+		case SimuJointIndex::TorsoPitch : return WaistSag_id;
+		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
+
+		// right arm
+		case SimuJointIndex::RightShPitch  : return RShSag_id;
+		case SimuJointIndex::RightShRoll   : return RShLat_id;
+		case SimuJointIndex::RightShYaw    : return RShYaw_id;
+		case SimuJointIndex::RightElbPitch : return RElb_id;
+
+		// left arm
+		case SimuJointIndex::LeftShPitch  : return LShSag_id;
+		case SimuJointIndex::LeftShRoll   : return LShLat_id;
+		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
+		case SimuJointIndex::LeftElbPitch : return LElb_id;
+	
+		default:
+			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the bodies
+ * 
+ * \param[in] body_id ID of the requested body
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsBodyId(int body_id)
+{
+	switch (body_id)
+	{
+		case SimuBodyIndex::Waist : return Waist_id;
+
+		case SimuBodyIndex::RHipmot : return RHipmot_id;
+		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
+		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
+		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
+		case SimuBodyIndex::RFootmot : return RFootmot_id;
+		case SimuBodyIndex::RFoot    : return RFoot_id;
+
+		case SimuBodyIndex::LHipmot : return LHipmot_id;
+		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
+		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
+		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
+		case SimuBodyIndex::LFootmot : return LFootmot_id;
+		case SimuBodyIndex::LFoot    : return LFoot_id;
+
+		case SimuBodyIndex::DWL : return DWL_id;
+		case SimuBodyIndex::DWS : return DWS_id;
+		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
+
+		case SimuBodyIndex::RShp : return RShp_id;
+		case SimuBodyIndex::RShr : return RShr_id;
+		case SimuBodyIndex::RShy : return RShy_id;
+		case SimuBodyIndex::RElb : return RElb_id;
+		case SimuBodyIndex::LShp : return LShp_id;
+		case SimuBodyIndex::LShr : return LShr_id;
+		case SimuBodyIndex::LShy : return LShy_id;
+		case SimuBodyIndex::LElb : return LElb_id;
+	
+		default:
+				std::cout << "Error: unknown body index: " << body_id << std::endl;
+				exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the S sensors
+ * 
+ * \param[in] Ssens_id ID of the requested S sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
+{
+	switch (Ssens_id)
+	{
+		// mid-waist
+		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
+
+		// one part feet
+		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
+		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
+
+		// legs
+		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
+		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
+		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
+		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
+
+		// torso
+		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
+
+		// arms
+		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
+		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
+		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
+		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
+
+		// com
+		case SimuSsensIndex::WaistCg : return WaistCg_id;
+
+		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
+		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
+		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
+
+		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
+		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
+		case SimuSsensIndex::RFootCg    : return RFootCg_id;
+
+		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
+		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
+		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
+
+		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
+		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
+		case SimuSsensIndex::LFootCg    : return LFootCg_id;
+
+		case SimuSsensIndex::DWLCg : return DWLCg_id;
+		case SimuSsensIndex::DWSCg : return DWSCg_id;
+		case SimuSsensIndex::DWYCg : return DWYCg_id;
+
+		case SimuSsensIndex::RShpCg : return RShpCg_id;
+		case SimuSsensIndex::RShrCg : return RShrCg_id;
+		case SimuSsensIndex::RShyCg : return RShyCg_id;
+		case SimuSsensIndex::RElbCg : return RElbCg_id;
+
+		case SimuSsensIndex::LShpCg : return LShpCg_id;
+		case SimuSsensIndex::LShrCg : return LShrCg_id;
+		case SimuSsensIndex::LShyCg : return LShyCg_id;
+		case SimuSsensIndex::LElbCg : return LElbCg_id;
+	
+		default:
+			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the F sensors
+ * 
+ * \param[in] Fsens_id ID of the requested F sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
+{
+	switch (Fsens_id)
+	{
+		// mid-waist
+		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
+
+		// legs
+		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
+		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
+		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
+		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
+
+		// one part feet
+		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
+		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
+
+		// torso
+		case SimuFsensIndex::Torso : return Torso_contact_force_id;
+
+		// arms
+		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
+		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
+		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
+		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
+	
+		default:
+			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the links
+ * 
+ * \param[in] link_id ID of the requested link
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsLinkId(int link_id)
+{
+	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
+	exit(EXIT_FAILURE);
+
+	return -1;
+}
diff --git a/dataR/extra/init_feet/user_kinematics_model.c b/dataR/extra/init_feet/user_kinematics_model.c
new file mode 100644
index 0000000..542f5ca
--- /dev/null
+++ b/dataR/extra/init_feet/user_kinematics_model.c
@@ -0,0 +1,1734 @@
+#include "mbs_data.h"
+
+#include "math.h"
+
+/*! \brief compute the kinematics model of sensors with ExtForces
+ * 
+ * \param[in] mbs_data Robotran structure
+ * \param[out] PxF_tab tabular with all the 'PxF' computed
+ * \param[out] RxF_tab tabular with all the 'RxF' computed
+ * \param[out] VxF_tab tabular with all the 'VxF' computed
+ * \param[out] OMxF_tab tabular with all the 'OMxF' computed
+ */
+void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
+{
+    // variables declaration
+    int i, j;
+    double *q, *qd;
+    double **dpt;
+
+    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
+    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
+    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
+    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
+
+    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
+    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
+
+    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
+    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
+    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
+    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
+    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
+    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
+    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
+    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
+    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
+    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
+    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
+    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
+    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
+    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
+    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
+    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
+    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
+    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
+    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
+    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
+    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
+    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
+    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
+    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
+    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
+    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
+    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
+    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
+    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
+    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
+    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
+    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
+    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
+    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
+    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
+    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
+    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
+    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
+    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
+    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
+    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
+    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
+    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
+    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
+    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
+    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
+    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
+    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
+    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
+    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
+    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
+    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
+    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
+    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
+    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
+    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
+    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
+    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
+    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
+    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
+    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
+    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
+    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
+    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
+    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
+    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
+    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
+    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
+    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
+    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
+    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
+    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
+    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
+    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
+    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
+    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
+    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
+    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
+    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
+    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
+    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
+    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
+    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
+    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
+    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
+
+    // variables initialization
+    q   = mbs_data->q;
+    qd  = mbs_data->qd;
+    dpt = mbs_data->dpt;
+
+    // cosines and sines
+    C4 = cos(q[4]);
+    C5 = cos(q[5]);
+    C6 = cos(q[6]);
+    C7 = cos(q[7]);
+    C8 = cos(q[8]);
+    C9 = cos(q[9]);
+    C10 = cos(q[10]);
+    C11 = cos(q[11]);
+    C12 = cos(q[12]);
+    C13 = cos(q[13]);
+    C14 = cos(q[14]);
+    C15 = cos(q[15]);
+    C16 = cos(q[16]);
+    C17 = cos(q[17]);
+    C18 = cos(q[18]);
+    C19 = cos(q[19]);
+    C20 = cos(q[20]);
+    C21 = cos(q[21]);
+    C22 = cos(q[22]);
+    C23 = cos(q[23]);
+    C24 = cos(q[24]);
+    C25 = cos(q[25]);
+    C26 = cos(q[26]);
+    C27 = cos(q[27]);
+    C28 = cos(q[28]);
+    C29 = cos(q[29]);
+    S4 = sin(q[4]);
+    S5 = sin(q[5]);
+    S6 = sin(q[6]);
+    S7 = sin(q[7]);
+    S8 = sin(q[8]);
+    S9 = sin(q[9]);
+    S10 = sin(q[10]);
+    S11 = sin(q[11]);
+    S12 = sin(q[12]);
+    S13 = sin(q[13]);
+    S14 = sin(q[14]);
+    S15 = sin(q[15]);
+    S16 = sin(q[16]);
+    S17 = sin(q[17]);
+    S18 = sin(q[18]);
+    S19 = sin(q[19]);
+    S20 = sin(q[20]);
+    S21 = sin(q[21]);
+    S22 = sin(q[22]);
+    S23 = sin(q[23]);
+    S24 = sin(q[24]);
+    S25 = sin(q[25]);
+    S26 = sin(q[26]);
+    S27 = sin(q[27]);
+    S28 = sin(q[28]);
+    S29 = sin(q[29]);
+
+    // main computation
+    ROcp25_25 = S4*S5;
+    ROcp25_35 = -C4*S5;
+    ROcp25_85 = -S4*C5;
+    ROcp25_95 = C4*C5;
+    ROcp25_16 = C5*C6;
+    ROcp25_26 = ROcp25_25*C6+C4*S6;
+    ROcp25_36 = ROcp25_35*C6+S4*S6;
+    ROcp25_46 = -C5*S6;
+    ROcp25_56 = -ROcp25_25*S6+C4*C6;
+    ROcp25_66 = -ROcp25_35*S6+S4*C6;
+    OMcp25_25 = qd[5]*C4;
+    OMcp25_35 = qd[5]*S4;
+    OMcp25_16 = qd[4]+qd[6]*S5;
+    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
+    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
+    PxF1[1] = q[1];
+    PxF1[2] = q[2];
+    PxF1[3] = q[3];
+    RxF1[1][1] = ROcp25_16;
+    RxF1[1][2] = ROcp25_26;
+    RxF1[1][3] = ROcp25_36;
+    RxF1[2][1] = ROcp25_46;
+    RxF1[2][2] = ROcp25_56;
+    RxF1[2][3] = ROcp25_66;
+    RxF1[3][1] = S5;
+    RxF1[3][2] = ROcp25_85;
+    RxF1[3][3] = ROcp25_95;
+    VxF1[1] = qd[1];
+    VxF1[2] = qd[2];
+    VxF1[3] = qd[3];
+    OMxF1[1] = OMcp25_16;
+    OMxF1[2] = OMcp25_26;
+    OMxF1[3] = OMcp25_36;
+    ROcp26_25 = S4*S5;
+    ROcp26_35 = -C4*S5;
+    ROcp26_85 = -S4*C5;
+    ROcp26_95 = C4*C5;
+    ROcp26_16 = C5*C6;
+    ROcp26_26 = ROcp26_25*C6+C4*S6;
+    ROcp26_36 = ROcp26_35*C6+S4*S6;
+    ROcp26_46 = -C5*S6;
+    ROcp26_56 = -ROcp26_25*S6+C4*C6;
+    ROcp26_66 = -ROcp26_35*S6+S4*C6;
+    ROcp26_17 = ROcp26_16*C7-S5*S7;
+    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
+    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
+    ROcp26_77 = ROcp26_16*S7+S5*C7;
+    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
+    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
+    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
+    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
+    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
+    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
+    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
+    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
+    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
+    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
+    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
+    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
+    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
+    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
+    OMcp26_25 = qd[5]*C4;
+    OMcp26_35 = qd[5]*S4;
+    OMcp26_16 = qd[4]+qd[6]*S5;
+    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
+    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
+    RLcp26_17 = ROcp26_46*dpt[2][2];
+    RLcp26_27 = ROcp26_56*dpt[2][2];
+    RLcp26_37 = ROcp26_66*dpt[2][2];
+    POcp26_17 = q[1]+RLcp26_17;
+    POcp26_27 = q[2]+RLcp26_27;
+    POcp26_37 = q[3]+RLcp26_37;
+    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
+    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
+    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
+    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
+    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
+    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
+    VIcp26_17 = qd[1]+ORcp26_17;
+    VIcp26_27 = qd[2]+ORcp26_27;
+    VIcp26_37 = qd[3]+ORcp26_37;
+    RLcp26_18 = ROcp26_46*dpt[2][6];
+    RLcp26_28 = ROcp26_56*dpt[2][6];
+    RLcp26_38 = ROcp26_66*dpt[2][6];
+    POcp26_18 = POcp26_17+RLcp26_18;
+    POcp26_28 = POcp26_27+RLcp26_28;
+    POcp26_38 = POcp26_37+RLcp26_38;
+    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
+    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
+    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
+    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
+    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
+    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
+    VIcp26_18 = ORcp26_18+VIcp26_17;
+    VIcp26_28 = ORcp26_28+VIcp26_27;
+    VIcp26_38 = ORcp26_38+VIcp26_37;
+    RLcp26_19 = ROcp26_78*dpt[3][8];
+    RLcp26_29 = ROcp26_88*dpt[3][8];
+    RLcp26_39 = ROcp26_98*dpt[3][8];
+    POcp26_19 = POcp26_18+RLcp26_19;
+    POcp26_29 = POcp26_28+RLcp26_29;
+    POcp26_39 = POcp26_38+RLcp26_39;
+    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
+    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
+    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
+    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
+    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
+    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
+    VIcp26_19 = ORcp26_19+VIcp26_18;
+    VIcp26_29 = ORcp26_29+VIcp26_28;
+    VIcp26_39 = ORcp26_39+VIcp26_38;
+    PxF2[1] = POcp26_19;
+    PxF2[2] = POcp26_29;
+    PxF2[3] = POcp26_39;
+    RxF2[1][1] = ROcp26_19;
+    RxF2[1][2] = ROcp26_29;
+    RxF2[1][3] = ROcp26_39;
+    RxF2[2][1] = ROcp26_49;
+    RxF2[2][2] = ROcp26_59;
+    RxF2[2][3] = ROcp26_69;
+    RxF2[3][1] = ROcp26_78;
+    RxF2[3][2] = ROcp26_88;
+    RxF2[3][3] = ROcp26_98;
+    VxF2[1] = VIcp26_19;
+    VxF2[2] = VIcp26_29;
+    VxF2[3] = VIcp26_39;
+    OMxF2[1] = OMcp26_19;
+    OMxF2[2] = OMcp26_29;
+    OMxF2[3] = OMcp26_39;
+    ROcp27_25 = S4*S5;
+    ROcp27_35 = -C4*S5;
+    ROcp27_85 = -S4*C5;
+    ROcp27_95 = C4*C5;
+    ROcp27_16 = C5*C6;
+    ROcp27_26 = ROcp27_25*C6+C4*S6;
+    ROcp27_36 = ROcp27_35*C6+S4*S6;
+    ROcp27_46 = -C5*S6;
+    ROcp27_56 = -ROcp27_25*S6+C4*C6;
+    ROcp27_66 = -ROcp27_35*S6+S4*C6;
+    ROcp27_17 = ROcp27_16*C7-S5*S7;
+    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
+    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
+    ROcp27_77 = ROcp27_16*S7+S5*C7;
+    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
+    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
+    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
+    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
+    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
+    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
+    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
+    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
+    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
+    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
+    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
+    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
+    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
+    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
+    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
+    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
+    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
+    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
+    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
+    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
+    OMcp27_25 = qd[5]*C4;
+    OMcp27_35 = qd[5]*S4;
+    OMcp27_16 = qd[4]+qd[6]*S5;
+    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
+    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
+    RLcp27_17 = ROcp27_46*dpt[2][2];
+    RLcp27_27 = ROcp27_56*dpt[2][2];
+    RLcp27_37 = ROcp27_66*dpt[2][2];
+    POcp27_17 = q[1]+RLcp27_17;
+    POcp27_27 = q[2]+RLcp27_27;
+    POcp27_37 = q[3]+RLcp27_37;
+    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
+    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
+    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
+    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
+    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
+    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
+    VIcp27_17 = qd[1]+ORcp27_17;
+    VIcp27_27 = qd[2]+ORcp27_27;
+    VIcp27_37 = qd[3]+ORcp27_37;
+    RLcp27_18 = ROcp27_46*dpt[2][6];
+    RLcp27_28 = ROcp27_56*dpt[2][6];
+    RLcp27_38 = ROcp27_66*dpt[2][6];
+    POcp27_18 = POcp27_17+RLcp27_18;
+    POcp27_28 = POcp27_27+RLcp27_28;
+    POcp27_38 = POcp27_37+RLcp27_38;
+    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
+    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
+    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
+    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
+    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
+    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
+    VIcp27_18 = ORcp27_18+VIcp27_17;
+    VIcp27_28 = ORcp27_28+VIcp27_27;
+    VIcp27_38 = ORcp27_38+VIcp27_37;
+    RLcp27_19 = ROcp27_78*dpt[3][8];
+    RLcp27_29 = ROcp27_88*dpt[3][8];
+    RLcp27_39 = ROcp27_98*dpt[3][8];
+    POcp27_19 = POcp27_18+RLcp27_19;
+    POcp27_29 = POcp27_28+RLcp27_29;
+    POcp27_39 = POcp27_38+RLcp27_39;
+    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
+    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
+    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
+    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
+    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
+    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
+    VIcp27_19 = ORcp27_19+VIcp27_18;
+    VIcp27_29 = ORcp27_29+VIcp27_28;
+    VIcp27_39 = ORcp27_39+VIcp27_38;
+    RLcp27_110 = ROcp27_78*dpt[3][11];
+    RLcp27_210 = ROcp27_88*dpt[3][11];
+    RLcp27_310 = ROcp27_98*dpt[3][11];
+    POcp27_110 = POcp27_19+RLcp27_110;
+    POcp27_210 = POcp27_29+RLcp27_210;
+    POcp27_310 = POcp27_39+RLcp27_310;
+    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
+    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
+    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
+    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
+    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
+    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
+    VIcp27_110 = ORcp27_110+VIcp27_19;
+    VIcp27_210 = ORcp27_210+VIcp27_29;
+    VIcp27_310 = ORcp27_310+VIcp27_39;
+    PxF3[1] = POcp27_110;
+    PxF3[2] = POcp27_210;
+    PxF3[3] = POcp27_310;
+    RxF3[1][1] = ROcp27_110;
+    RxF3[1][2] = ROcp27_210;
+    RxF3[1][3] = ROcp27_310;
+    RxF3[2][1] = ROcp27_49;
+    RxF3[2][2] = ROcp27_59;
+    RxF3[2][3] = ROcp27_69;
+    RxF3[3][1] = ROcp27_710;
+    RxF3[3][2] = ROcp27_810;
+    RxF3[3][3] = ROcp27_910;
+    VxF3[1] = VIcp27_110;
+    VxF3[2] = VIcp27_210;
+    VxF3[3] = VIcp27_310;
+    OMxF3[1] = OMcp27_110;
+    OMxF3[2] = OMcp27_210;
+    OMxF3[3] = OMcp27_310;
+    ROcp28_25 = S4*S5;
+    ROcp28_35 = -C4*S5;
+    ROcp28_85 = -S4*C5;
+    ROcp28_95 = C4*C5;
+    ROcp28_16 = C5*C6;
+    ROcp28_26 = ROcp28_25*C6+C4*S6;
+    ROcp28_36 = ROcp28_35*C6+S4*S6;
+    ROcp28_46 = -C5*S6;
+    ROcp28_56 = -ROcp28_25*S6+C4*C6;
+    ROcp28_66 = -ROcp28_35*S6+S4*C6;
+    ROcp28_17 = ROcp28_16*C7-S5*S7;
+    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
+    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
+    ROcp28_77 = ROcp28_16*S7+S5*C7;
+    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
+    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
+    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
+    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
+    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
+    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
+    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
+    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
+    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
+    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
+    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
+    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
+    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
+    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
+    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
+    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
+    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
+    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
+    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
+    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
+    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
+    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
+    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
+    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
+    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
+    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
+    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
+    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
+    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
+    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
+    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
+    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
+    OMcp28_25 = qd[5]*C4;
+    OMcp28_35 = qd[5]*S4;
+    OMcp28_16 = qd[4]+qd[6]*S5;
+    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
+    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
+    RLcp28_17 = ROcp28_46*dpt[2][2];
+    RLcp28_27 = ROcp28_56*dpt[2][2];
+    RLcp28_37 = ROcp28_66*dpt[2][2];
+    POcp28_17 = q[1]+RLcp28_17;
+    POcp28_27 = q[2]+RLcp28_27;
+    POcp28_37 = q[3]+RLcp28_37;
+    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
+    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
+    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
+    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
+    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
+    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
+    VIcp28_17 = qd[1]+ORcp28_17;
+    VIcp28_27 = qd[2]+ORcp28_27;
+    VIcp28_37 = qd[3]+ORcp28_37;
+    RLcp28_18 = ROcp28_46*dpt[2][6];
+    RLcp28_28 = ROcp28_56*dpt[2][6];
+    RLcp28_38 = ROcp28_66*dpt[2][6];
+    POcp28_18 = POcp28_17+RLcp28_18;
+    POcp28_28 = POcp28_27+RLcp28_28;
+    POcp28_38 = POcp28_37+RLcp28_38;
+    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
+    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
+    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
+    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
+    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
+    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
+    VIcp28_18 = ORcp28_18+VIcp28_17;
+    VIcp28_28 = ORcp28_28+VIcp28_27;
+    VIcp28_38 = ORcp28_38+VIcp28_37;
+    RLcp28_19 = ROcp28_78*dpt[3][8];
+    RLcp28_29 = ROcp28_88*dpt[3][8];
+    RLcp28_39 = ROcp28_98*dpt[3][8];
+    POcp28_19 = POcp28_18+RLcp28_19;
+    POcp28_29 = POcp28_28+RLcp28_29;
+    POcp28_39 = POcp28_38+RLcp28_39;
+    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
+    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
+    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
+    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
+    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
+    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
+    VIcp28_19 = ORcp28_19+VIcp28_18;
+    VIcp28_29 = ORcp28_29+VIcp28_28;
+    VIcp28_39 = ORcp28_39+VIcp28_38;
+    RLcp28_110 = ROcp28_78*dpt[3][11];
+    RLcp28_210 = ROcp28_88*dpt[3][11];
+    RLcp28_310 = ROcp28_98*dpt[3][11];
+    POcp28_110 = POcp28_19+RLcp28_110;
+    POcp28_210 = POcp28_29+RLcp28_210;
+    POcp28_310 = POcp28_39+RLcp28_310;
+    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
+    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
+    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
+    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
+    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
+    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
+    VIcp28_110 = ORcp28_110+VIcp28_19;
+    VIcp28_210 = ORcp28_210+VIcp28_29;
+    VIcp28_310 = ORcp28_310+VIcp28_39;
+    RLcp28_111 = ROcp28_710*dpt[3][14];
+    RLcp28_211 = ROcp28_810*dpt[3][14];
+    RLcp28_311 = ROcp28_910*dpt[3][14];
+    POcp28_111 = POcp28_110+RLcp28_111;
+    POcp28_211 = POcp28_210+RLcp28_211;
+    POcp28_311 = POcp28_310+RLcp28_311;
+    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
+    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
+    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
+    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
+    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
+    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
+    VIcp28_111 = ORcp28_111+VIcp28_110;
+    VIcp28_211 = ORcp28_211+VIcp28_210;
+    VIcp28_311 = ORcp28_311+VIcp28_310;
+    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
+    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
+    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
+    RLcp28_113 = ROcp28_712*dpt[3][18];
+    RLcp28_213 = ROcp28_812*dpt[3][18];
+    RLcp28_313 = ROcp28_912*dpt[3][18];
+    POcp28_113 = POcp28_111+RLcp28_113;
+    POcp28_213 = POcp28_211+RLcp28_213;
+    POcp28_313 = POcp28_311+RLcp28_313;
+    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
+    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
+    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
+    VIcp28_113 = ORcp28_113+VIcp28_111;
+    VIcp28_213 = ORcp28_213+VIcp28_211;
+    VIcp28_313 = ORcp28_313+VIcp28_311;
+    PxF4[1] = POcp28_113;
+    PxF4[2] = POcp28_213;
+    PxF4[3] = POcp28_313;
+    RxF4[1][1] = ROcp28_112;
+    RxF4[1][2] = ROcp28_212;
+    RxF4[1][3] = ROcp28_312;
+    RxF4[2][1] = ROcp28_411;
+    RxF4[2][2] = ROcp28_511;
+    RxF4[2][3] = ROcp28_611;
+    RxF4[3][1] = ROcp28_712;
+    RxF4[3][2] = ROcp28_812;
+    RxF4[3][3] = ROcp28_912;
+    VxF4[1] = VIcp28_113;
+    VxF4[2] = VIcp28_213;
+    VxF4[3] = VIcp28_313;
+    OMxF4[1] = OMcp28_112;
+    OMxF4[2] = OMcp28_212;
+    OMxF4[3] = OMcp28_312;
+    ROcp29_25 = S4*S5;
+    ROcp29_35 = -C4*S5;
+    ROcp29_85 = -S4*C5;
+    ROcp29_95 = C4*C5;
+    ROcp29_16 = C5*C6;
+    ROcp29_26 = ROcp29_25*C6+C4*S6;
+    ROcp29_36 = ROcp29_35*C6+S4*S6;
+    ROcp29_46 = -C5*S6;
+    ROcp29_56 = -ROcp29_25*S6+C4*C6;
+    ROcp29_66 = -ROcp29_35*S6+S4*C6;
+    ROcp29_113 = ROcp29_16*C13-S13*S5;
+    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
+    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
+    ROcp29_713 = ROcp29_16*S13+C13*S5;
+    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
+    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
+    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
+    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
+    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
+    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
+    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
+    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
+    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
+    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
+    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
+    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
+    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
+    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
+    OMcp29_25 = qd[5]*C4;
+    OMcp29_35 = qd[5]*S4;
+    OMcp29_16 = qd[4]+qd[6]*S5;
+    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
+    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
+    RLcp29_17 = ROcp29_46*dpt[2][3];
+    RLcp29_27 = ROcp29_56*dpt[2][3];
+    RLcp29_37 = ROcp29_66*dpt[2][3];
+    POcp29_17 = q[1]+RLcp29_17;
+    POcp29_27 = q[2]+RLcp29_27;
+    POcp29_37 = q[3]+RLcp29_37;
+    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
+    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
+    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
+    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
+    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
+    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
+    VIcp29_17 = qd[1]+ORcp29_17;
+    VIcp29_27 = qd[2]+ORcp29_27;
+    VIcp29_37 = qd[3]+ORcp29_37;
+    RLcp29_18 = ROcp29_46*dpt[2][20];
+    RLcp29_28 = ROcp29_56*dpt[2][20];
+    RLcp29_38 = ROcp29_66*dpt[2][20];
+    POcp29_18 = POcp29_17+RLcp29_18;
+    POcp29_28 = POcp29_27+RLcp29_28;
+    POcp29_38 = POcp29_37+RLcp29_38;
+    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
+    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
+    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
+    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
+    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
+    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
+    VIcp29_18 = ORcp29_18+VIcp29_17;
+    VIcp29_28 = ORcp29_28+VIcp29_27;
+    VIcp29_38 = ORcp29_38+VIcp29_37;
+    RLcp29_19 = ROcp29_714*dpt[3][22];
+    RLcp29_29 = ROcp29_814*dpt[3][22];
+    RLcp29_39 = ROcp29_914*dpt[3][22];
+    POcp29_19 = POcp29_18+RLcp29_19;
+    POcp29_29 = POcp29_28+RLcp29_29;
+    POcp29_39 = POcp29_38+RLcp29_39;
+    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
+    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
+    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
+    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
+    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
+    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
+    VIcp29_19 = ORcp29_19+VIcp29_18;
+    VIcp29_29 = ORcp29_29+VIcp29_28;
+    VIcp29_39 = ORcp29_39+VIcp29_38;
+    PxF5[1] = POcp29_19;
+    PxF5[2] = POcp29_29;
+    PxF5[3] = POcp29_39;
+    RxF5[1][1] = ROcp29_115;
+    RxF5[1][2] = ROcp29_215;
+    RxF5[1][3] = ROcp29_315;
+    RxF5[2][1] = ROcp29_415;
+    RxF5[2][2] = ROcp29_515;
+    RxF5[2][3] = ROcp29_615;
+    RxF5[3][1] = ROcp29_714;
+    RxF5[3][2] = ROcp29_814;
+    RxF5[3][3] = ROcp29_914;
+    VxF5[1] = VIcp29_19;
+    VxF5[2] = VIcp29_29;
+    VxF5[3] = VIcp29_39;
+    OMxF5[1] = OMcp29_19;
+    OMxF5[2] = OMcp29_29;
+    OMxF5[3] = OMcp29_39;
+    ROcp30_25 = S4*S5;
+    ROcp30_35 = -C4*S5;
+    ROcp30_85 = -S4*C5;
+    ROcp30_95 = C4*C5;
+    ROcp30_16 = C5*C6;
+    ROcp30_26 = ROcp30_25*C6+C4*S6;
+    ROcp30_36 = ROcp30_35*C6+S4*S6;
+    ROcp30_46 = -C5*S6;
+    ROcp30_56 = -ROcp30_25*S6+C4*C6;
+    ROcp30_66 = -ROcp30_35*S6+S4*C6;
+    ROcp30_113 = ROcp30_16*C13-S13*S5;
+    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
+    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
+    ROcp30_713 = ROcp30_16*S13+C13*S5;
+    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
+    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
+    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
+    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
+    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
+    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
+    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
+    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
+    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
+    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
+    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
+    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
+    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
+    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
+    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
+    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
+    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
+    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
+    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
+    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
+    OMcp30_25 = qd[5]*C4;
+    OMcp30_35 = qd[5]*S4;
+    OMcp30_16 = qd[4]+qd[6]*S5;
+    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
+    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
+    RLcp30_17 = ROcp30_46*dpt[2][3];
+    RLcp30_27 = ROcp30_56*dpt[2][3];
+    RLcp30_37 = ROcp30_66*dpt[2][3];
+    POcp30_17 = q[1]+RLcp30_17;
+    POcp30_27 = q[2]+RLcp30_27;
+    POcp30_37 = q[3]+RLcp30_37;
+    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
+    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
+    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
+    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
+    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
+    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
+    VIcp30_17 = qd[1]+ORcp30_17;
+    VIcp30_27 = qd[2]+ORcp30_27;
+    VIcp30_37 = qd[3]+ORcp30_37;
+    RLcp30_18 = ROcp30_46*dpt[2][20];
+    RLcp30_28 = ROcp30_56*dpt[2][20];
+    RLcp30_38 = ROcp30_66*dpt[2][20];
+    POcp30_18 = POcp30_17+RLcp30_18;
+    POcp30_28 = POcp30_27+RLcp30_28;
+    POcp30_38 = POcp30_37+RLcp30_38;
+    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
+    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
+    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
+    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
+    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
+    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
+    VIcp30_18 = ORcp30_18+VIcp30_17;
+    VIcp30_28 = ORcp30_28+VIcp30_27;
+    VIcp30_38 = ORcp30_38+VIcp30_37;
+    RLcp30_19 = ROcp30_714*dpt[3][22];
+    RLcp30_29 = ROcp30_814*dpt[3][22];
+    RLcp30_39 = ROcp30_914*dpt[3][22];
+    POcp30_19 = POcp30_18+RLcp30_19;
+    POcp30_29 = POcp30_28+RLcp30_29;
+    POcp30_39 = POcp30_38+RLcp30_39;
+    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
+    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
+    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
+    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
+    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
+    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
+    VIcp30_19 = ORcp30_19+VIcp30_18;
+    VIcp30_29 = ORcp30_29+VIcp30_28;
+    VIcp30_39 = ORcp30_39+VIcp30_38;
+    RLcp30_110 = ROcp30_714*dpt[3][25];
+    RLcp30_210 = ROcp30_814*dpt[3][25];
+    RLcp30_310 = ROcp30_914*dpt[3][25];
+    POcp30_110 = POcp30_19+RLcp30_110;
+    POcp30_210 = POcp30_29+RLcp30_210;
+    POcp30_310 = POcp30_39+RLcp30_310;
+    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
+    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
+    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
+    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
+    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
+    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
+    VIcp30_110 = ORcp30_110+VIcp30_19;
+    VIcp30_210 = ORcp30_210+VIcp30_29;
+    VIcp30_310 = ORcp30_310+VIcp30_39;
+    PxF6[1] = POcp30_110;
+    PxF6[2] = POcp30_210;
+    PxF6[3] = POcp30_310;
+    RxF6[1][1] = ROcp30_116;
+    RxF6[1][2] = ROcp30_216;
+    RxF6[1][3] = ROcp30_316;
+    RxF6[2][1] = ROcp30_415;
+    RxF6[2][2] = ROcp30_515;
+    RxF6[2][3] = ROcp30_615;
+    RxF6[3][1] = ROcp30_716;
+    RxF6[3][2] = ROcp30_816;
+    RxF6[3][3] = ROcp30_916;
+    VxF6[1] = VIcp30_110;
+    VxF6[2] = VIcp30_210;
+    VxF6[3] = VIcp30_310;
+    OMxF6[1] = OMcp30_110;
+    OMxF6[2] = OMcp30_210;
+    OMxF6[3] = OMcp30_310;
+    ROcp31_25 = S4*S5;
+    ROcp31_35 = -C4*S5;
+    ROcp31_85 = -S4*C5;
+    ROcp31_95 = C4*C5;
+    ROcp31_16 = C5*C6;
+    ROcp31_26 = ROcp31_25*C6+C4*S6;
+    ROcp31_36 = ROcp31_35*C6+S4*S6;
+    ROcp31_46 = -C5*S6;
+    ROcp31_56 = -ROcp31_25*S6+C4*C6;
+    ROcp31_66 = -ROcp31_35*S6+S4*C6;
+    ROcp31_113 = ROcp31_16*C13-S13*S5;
+    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
+    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
+    ROcp31_713 = ROcp31_16*S13+C13*S5;
+    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
+    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
+    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
+    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
+    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
+    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
+    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
+    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
+    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
+    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
+    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
+    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
+    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
+    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
+    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
+    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
+    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
+    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
+    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
+    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
+    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
+    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
+    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
+    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
+    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
+    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
+    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
+    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
+    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
+    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
+    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
+    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
+    OMcp31_25 = qd[5]*C4;
+    OMcp31_35 = qd[5]*S4;
+    OMcp31_16 = qd[4]+qd[6]*S5;
+    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
+    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
+    RLcp31_17 = ROcp31_46*dpt[2][3];
+    RLcp31_27 = ROcp31_56*dpt[2][3];
+    RLcp31_37 = ROcp31_66*dpt[2][3];
+    POcp31_17 = q[1]+RLcp31_17;
+    POcp31_27 = q[2]+RLcp31_27;
+    POcp31_37 = q[3]+RLcp31_37;
+    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
+    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
+    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
+    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
+    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
+    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
+    VIcp31_17 = qd[1]+ORcp31_17;
+    VIcp31_27 = qd[2]+ORcp31_27;
+    VIcp31_37 = qd[3]+ORcp31_37;
+    RLcp31_18 = ROcp31_46*dpt[2][20];
+    RLcp31_28 = ROcp31_56*dpt[2][20];
+    RLcp31_38 = ROcp31_66*dpt[2][20];
+    POcp31_18 = POcp31_17+RLcp31_18;
+    POcp31_28 = POcp31_27+RLcp31_28;
+    POcp31_38 = POcp31_37+RLcp31_38;
+    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
+    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
+    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
+    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
+    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
+    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
+    VIcp31_18 = ORcp31_18+VIcp31_17;
+    VIcp31_28 = ORcp31_28+VIcp31_27;
+    VIcp31_38 = ORcp31_38+VIcp31_37;
+    RLcp31_19 = ROcp31_714*dpt[3][22];
+    RLcp31_29 = ROcp31_814*dpt[3][22];
+    RLcp31_39 = ROcp31_914*dpt[3][22];
+    POcp31_19 = POcp31_18+RLcp31_19;
+    POcp31_29 = POcp31_28+RLcp31_29;
+    POcp31_39 = POcp31_38+RLcp31_39;
+    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
+    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
+    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
+    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
+    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
+    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
+    VIcp31_19 = ORcp31_19+VIcp31_18;
+    VIcp31_29 = ORcp31_29+VIcp31_28;
+    VIcp31_39 = ORcp31_39+VIcp31_38;
+    RLcp31_110 = ROcp31_714*dpt[3][25];
+    RLcp31_210 = ROcp31_814*dpt[3][25];
+    RLcp31_310 = ROcp31_914*dpt[3][25];
+    POcp31_110 = POcp31_19+RLcp31_110;
+    POcp31_210 = POcp31_29+RLcp31_210;
+    POcp31_310 = POcp31_39+RLcp31_310;
+    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
+    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
+    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
+    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
+    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
+    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
+    VIcp31_110 = ORcp31_110+VIcp31_19;
+    VIcp31_210 = ORcp31_210+VIcp31_29;
+    VIcp31_310 = ORcp31_310+VIcp31_39;
+    RLcp31_111 = ROcp31_716*dpt[3][28];
+    RLcp31_211 = ROcp31_816*dpt[3][28];
+    RLcp31_311 = ROcp31_916*dpt[3][28];
+    POcp31_111 = POcp31_110+RLcp31_111;
+    POcp31_211 = POcp31_210+RLcp31_211;
+    POcp31_311 = POcp31_310+RLcp31_311;
+    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
+    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
+    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
+    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
+    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
+    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
+    VIcp31_111 = ORcp31_111+VIcp31_110;
+    VIcp31_211 = ORcp31_211+VIcp31_210;
+    VIcp31_311 = ORcp31_311+VIcp31_310;
+    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
+    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
+    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
+    RLcp31_113 = ROcp31_718*dpt[3][33];
+    RLcp31_213 = ROcp31_818*dpt[3][33];
+    RLcp31_313 = ROcp31_918*dpt[3][33];
+    POcp31_113 = POcp31_111+RLcp31_113;
+    POcp31_213 = POcp31_211+RLcp31_213;
+    POcp31_313 = POcp31_311+RLcp31_313;
+    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
+    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
+    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
+    VIcp31_113 = ORcp31_113+VIcp31_111;
+    VIcp31_213 = ORcp31_213+VIcp31_211;
+    VIcp31_313 = ORcp31_313+VIcp31_311;
+    PxF7[1] = POcp31_113;
+    PxF7[2] = POcp31_213;
+    PxF7[3] = POcp31_313;
+    RxF7[1][1] = ROcp31_118;
+    RxF7[1][2] = ROcp31_218;
+    RxF7[1][3] = ROcp31_318;
+    RxF7[2][1] = ROcp31_417;
+    RxF7[2][2] = ROcp31_517;
+    RxF7[2][3] = ROcp31_617;
+    RxF7[3][1] = ROcp31_718;
+    RxF7[3][2] = ROcp31_818;
+    RxF7[3][3] = ROcp31_918;
+    VxF7[1] = VIcp31_113;
+    VxF7[2] = VIcp31_213;
+    VxF7[3] = VIcp31_313;
+    OMxF7[1] = OMcp31_112;
+    OMxF7[2] = OMcp31_212;
+    OMxF7[3] = OMcp31_312;
+    ROcp32_25 = S4*S5;
+    ROcp32_35 = -C4*S5;
+    ROcp32_85 = -S4*C5;
+    ROcp32_95 = C4*C5;
+    ROcp32_16 = C5*C6;
+    ROcp32_26 = ROcp32_25*C6+C4*S6;
+    ROcp32_36 = ROcp32_35*C6+S4*S6;
+    ROcp32_46 = -C5*S6;
+    ROcp32_56 = -ROcp32_25*S6+C4*C6;
+    ROcp32_66 = -ROcp32_35*S6+S4*C6;
+    ROcp32_419 = ROcp32_46*C19+S19*S5;
+    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
+    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
+    ROcp32_719 = -ROcp32_46*S19+C19*S5;
+    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
+    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
+    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
+    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
+    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
+    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
+    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
+    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
+    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
+    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
+    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
+    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
+    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
+    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
+    OMcp32_25 = qd[5]*C4;
+    OMcp32_35 = qd[5]*S4;
+    OMcp32_16 = qd[4]+qd[6]*S5;
+    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
+    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
+    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
+    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
+    POcp32_17 = q[1]+RLcp32_17;
+    POcp32_27 = q[2]+RLcp32_27;
+    POcp32_37 = q[3]+RLcp32_37;
+    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
+    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
+    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
+    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
+    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
+    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
+    VIcp32_17 = qd[1]+ORcp32_17;
+    VIcp32_27 = qd[2]+ORcp32_27;
+    VIcp32_37 = qd[3]+ORcp32_37;
+    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
+    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
+    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
+    RLcp32_19 = ROcp32_720*dpt[3][36];
+    RLcp32_29 = ROcp32_820*dpt[3][36];
+    RLcp32_39 = ROcp32_920*dpt[3][36];
+    POcp32_19 = POcp32_17+RLcp32_19;
+    POcp32_29 = POcp32_27+RLcp32_29;
+    POcp32_39 = POcp32_37+RLcp32_39;
+    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
+    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
+    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
+    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
+    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
+    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
+    VIcp32_19 = ORcp32_19+VIcp32_17;
+    VIcp32_29 = ORcp32_29+VIcp32_27;
+    VIcp32_39 = ORcp32_39+VIcp32_37;
+    PxF8[1] = POcp32_19;
+    PxF8[2] = POcp32_29;
+    PxF8[3] = POcp32_39;
+    RxF8[1][1] = ROcp32_121;
+    RxF8[1][2] = ROcp32_221;
+    RxF8[1][3] = ROcp32_321;
+    RxF8[2][1] = ROcp32_421;
+    RxF8[2][2] = ROcp32_521;
+    RxF8[2][3] = ROcp32_621;
+    RxF8[3][1] = ROcp32_720;
+    RxF8[3][2] = ROcp32_820;
+    RxF8[3][3] = ROcp32_920;
+    VxF8[1] = VIcp32_19;
+    VxF8[2] = VIcp32_29;
+    VxF8[3] = VIcp32_39;
+    OMxF8[1] = OMcp32_19;
+    OMxF8[2] = OMcp32_29;
+    OMxF8[3] = OMcp32_39;
+    ROcp33_25 = S4*S5;
+    ROcp33_35 = -C4*S5;
+    ROcp33_85 = -S4*C5;
+    ROcp33_95 = C4*C5;
+    ROcp33_16 = C5*C6;
+    ROcp33_26 = ROcp33_25*C6+C4*S6;
+    ROcp33_36 = ROcp33_35*C6+S4*S6;
+    ROcp33_46 = -C5*S6;
+    ROcp33_56 = -ROcp33_25*S6+C4*C6;
+    ROcp33_66 = -ROcp33_35*S6+S4*C6;
+    ROcp33_419 = ROcp33_46*C19+S19*S5;
+    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
+    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
+    ROcp33_719 = -ROcp33_46*S19+C19*S5;
+    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
+    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
+    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
+    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
+    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
+    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
+    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
+    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
+    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
+    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
+    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
+    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
+    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
+    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
+    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
+    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
+    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
+    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
+    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
+    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
+    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
+    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
+    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
+    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
+    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
+    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
+    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
+    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
+    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
+    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
+    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
+    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
+    OMcp33_25 = qd[5]*C4;
+    OMcp33_35 = qd[5]*S4;
+    OMcp33_16 = qd[4]+qd[6]*S5;
+    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
+    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
+    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
+    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
+    POcp33_17 = q[1]+RLcp33_17;
+    POcp33_27 = q[2]+RLcp33_27;
+    POcp33_37 = q[3]+RLcp33_37;
+    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
+    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
+    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
+    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
+    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
+    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
+    VIcp33_17 = qd[1]+ORcp33_17;
+    VIcp33_27 = qd[2]+ORcp33_27;
+    VIcp33_37 = qd[3]+ORcp33_37;
+    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
+    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
+    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
+    RLcp33_19 = ROcp33_720*dpt[3][36];
+    RLcp33_29 = ROcp33_820*dpt[3][36];
+    RLcp33_39 = ROcp33_920*dpt[3][36];
+    POcp33_19 = POcp33_17+RLcp33_19;
+    POcp33_29 = POcp33_27+RLcp33_29;
+    POcp33_39 = POcp33_37+RLcp33_39;
+    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
+    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
+    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
+    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
+    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
+    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
+    VIcp33_19 = ORcp33_19+VIcp33_17;
+    VIcp33_29 = ORcp33_29+VIcp33_27;
+    VIcp33_39 = ORcp33_39+VIcp33_37;
+    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
+    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
+    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
+    POcp33_110 = POcp33_19+RLcp33_110;
+    POcp33_210 = POcp33_29+RLcp33_210;
+    POcp33_310 = POcp33_39+RLcp33_310;
+    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
+    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
+    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
+    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
+    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
+    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
+    VIcp33_110 = ORcp33_110+VIcp33_19;
+    VIcp33_210 = ORcp33_210+VIcp33_29;
+    VIcp33_310 = ORcp33_310+VIcp33_39;
+    RLcp33_111 = ROcp33_421*dpt[2][43];
+    RLcp33_211 = ROcp33_521*dpt[2][43];
+    RLcp33_311 = ROcp33_621*dpt[2][43];
+    POcp33_111 = POcp33_110+RLcp33_111;
+    POcp33_211 = POcp33_210+RLcp33_211;
+    POcp33_311 = POcp33_310+RLcp33_311;
+    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
+    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
+    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
+    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
+    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
+    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
+    VIcp33_111 = ORcp33_111+VIcp33_110;
+    VIcp33_211 = ORcp33_211+VIcp33_210;
+    VIcp33_311 = ORcp33_311+VIcp33_310;
+    RLcp33_112 = ROcp33_723*dpt[3][45];
+    RLcp33_212 = ROcp33_823*dpt[3][45];
+    RLcp33_312 = ROcp33_923*dpt[3][45];
+    POcp33_112 = POcp33_111+RLcp33_112;
+    POcp33_212 = POcp33_211+RLcp33_212;
+    POcp33_312 = POcp33_311+RLcp33_312;
+    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
+    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
+    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
+    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
+    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
+    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
+    VIcp33_112 = ORcp33_112+VIcp33_111;
+    VIcp33_212 = ORcp33_212+VIcp33_211;
+    VIcp33_312 = ORcp33_312+VIcp33_311;
+    PxF9[1] = POcp33_112;
+    PxF9[2] = POcp33_212;
+    PxF9[3] = POcp33_312;
+    RxF9[1][1] = ROcp33_124;
+    RxF9[1][2] = ROcp33_224;
+    RxF9[1][3] = ROcp33_324;
+    RxF9[2][1] = ROcp33_424;
+    RxF9[2][2] = ROcp33_524;
+    RxF9[2][3] = ROcp33_624;
+    RxF9[3][1] = ROcp33_723;
+    RxF9[3][2] = ROcp33_823;
+    RxF9[3][3] = ROcp33_923;
+    VxF9[1] = VIcp33_112;
+    VxF9[2] = VIcp33_212;
+    VxF9[3] = VIcp33_312;
+    OMxF9[1] = OMcp33_112;
+    OMxF9[2] = OMcp33_212;
+    OMxF9[3] = OMcp33_312;
+    ROcp34_25 = S4*S5;
+    ROcp34_35 = -C4*S5;
+    ROcp34_85 = -S4*C5;
+    ROcp34_95 = C4*C5;
+    ROcp34_16 = C5*C6;
+    ROcp34_26 = ROcp34_25*C6+C4*S6;
+    ROcp34_36 = ROcp34_35*C6+S4*S6;
+    ROcp34_46 = -C5*S6;
+    ROcp34_56 = -ROcp34_25*S6+C4*C6;
+    ROcp34_66 = -ROcp34_35*S6+S4*C6;
+    ROcp34_419 = ROcp34_46*C19+S19*S5;
+    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
+    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
+    ROcp34_719 = -ROcp34_46*S19+C19*S5;
+    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
+    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
+    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
+    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
+    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
+    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
+    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
+    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
+    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
+    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
+    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
+    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
+    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
+    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
+    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
+    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
+    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
+    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
+    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
+    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
+    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
+    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
+    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
+    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
+    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
+    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
+    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
+    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
+    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
+    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
+    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
+    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
+    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
+    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
+    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
+    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
+    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
+    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
+    OMcp34_25 = qd[5]*C4;
+    OMcp34_35 = qd[5]*S4;
+    OMcp34_16 = qd[4]+qd[6]*S5;
+    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
+    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
+    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
+    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
+    POcp34_17 = q[1]+RLcp34_17;
+    POcp34_27 = q[2]+RLcp34_27;
+    POcp34_37 = q[3]+RLcp34_37;
+    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
+    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
+    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
+    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
+    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
+    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
+    VIcp34_17 = qd[1]+ORcp34_17;
+    VIcp34_27 = qd[2]+ORcp34_27;
+    VIcp34_37 = qd[3]+ORcp34_37;
+    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
+    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
+    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
+    RLcp34_19 = ROcp34_720*dpt[3][36];
+    RLcp34_29 = ROcp34_820*dpt[3][36];
+    RLcp34_39 = ROcp34_920*dpt[3][36];
+    POcp34_19 = POcp34_17+RLcp34_19;
+    POcp34_29 = POcp34_27+RLcp34_29;
+    POcp34_39 = POcp34_37+RLcp34_39;
+    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
+    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
+    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
+    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
+    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
+    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
+    VIcp34_19 = ORcp34_19+VIcp34_17;
+    VIcp34_29 = ORcp34_29+VIcp34_27;
+    VIcp34_39 = ORcp34_39+VIcp34_37;
+    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
+    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
+    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
+    POcp34_110 = POcp34_19+RLcp34_110;
+    POcp34_210 = POcp34_29+RLcp34_210;
+    POcp34_310 = POcp34_39+RLcp34_310;
+    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
+    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
+    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
+    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
+    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
+    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
+    VIcp34_110 = ORcp34_110+VIcp34_19;
+    VIcp34_210 = ORcp34_210+VIcp34_29;
+    VIcp34_310 = ORcp34_310+VIcp34_39;
+    RLcp34_111 = ROcp34_421*dpt[2][43];
+    RLcp34_211 = ROcp34_521*dpt[2][43];
+    RLcp34_311 = ROcp34_621*dpt[2][43];
+    POcp34_111 = POcp34_110+RLcp34_111;
+    POcp34_211 = POcp34_210+RLcp34_211;
+    POcp34_311 = POcp34_310+RLcp34_311;
+    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
+    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
+    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
+    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
+    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
+    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
+    VIcp34_111 = ORcp34_111+VIcp34_110;
+    VIcp34_211 = ORcp34_211+VIcp34_210;
+    VIcp34_311 = ORcp34_311+VIcp34_310;
+    RLcp34_112 = ROcp34_723*dpt[3][45];
+    RLcp34_212 = ROcp34_823*dpt[3][45];
+    RLcp34_312 = ROcp34_923*dpt[3][45];
+    POcp34_112 = POcp34_111+RLcp34_112;
+    POcp34_212 = POcp34_211+RLcp34_212;
+    POcp34_312 = POcp34_311+RLcp34_312;
+    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
+    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
+    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
+    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
+    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
+    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
+    VIcp34_112 = ORcp34_112+VIcp34_111;
+    VIcp34_212 = ORcp34_212+VIcp34_211;
+    VIcp34_312 = ORcp34_312+VIcp34_311;
+    RLcp34_113 = ROcp34_723*dpt[3][48];
+    RLcp34_213 = ROcp34_823*dpt[3][48];
+    RLcp34_313 = ROcp34_923*dpt[3][48];
+    POcp34_113 = POcp34_112+RLcp34_113;
+    POcp34_213 = POcp34_212+RLcp34_213;
+    POcp34_313 = POcp34_312+RLcp34_313;
+    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
+    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
+    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
+    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
+    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
+    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
+    VIcp34_113 = ORcp34_113+VIcp34_112;
+    VIcp34_213 = ORcp34_213+VIcp34_212;
+    VIcp34_313 = ORcp34_313+VIcp34_312;
+    PxF10[1] = POcp34_113;
+    PxF10[2] = POcp34_213;
+    PxF10[3] = POcp34_313;
+    RxF10[1][1] = ROcp34_125;
+    RxF10[1][2] = ROcp34_225;
+    RxF10[1][3] = ROcp34_325;
+    RxF10[2][1] = ROcp34_424;
+    RxF10[2][2] = ROcp34_524;
+    RxF10[2][3] = ROcp34_624;
+    RxF10[3][1] = ROcp34_725;
+    RxF10[3][2] = ROcp34_825;
+    RxF10[3][3] = ROcp34_925;
+    VxF10[1] = VIcp34_113;
+    VxF10[2] = VIcp34_213;
+    VxF10[3] = VIcp34_313;
+    OMxF10[1] = OMcp34_113;
+    OMxF10[2] = OMcp34_213;
+    OMxF10[3] = OMcp34_313;
+    ROcp35_25 = S4*S5;
+    ROcp35_35 = -C4*S5;
+    ROcp35_85 = -S4*C5;
+    ROcp35_95 = C4*C5;
+    ROcp35_16 = C5*C6;
+    ROcp35_26 = ROcp35_25*C6+C4*S6;
+    ROcp35_36 = ROcp35_35*C6+S4*S6;
+    ROcp35_46 = -C5*S6;
+    ROcp35_56 = -ROcp35_25*S6+C4*C6;
+    ROcp35_66 = -ROcp35_35*S6+S4*C6;
+    ROcp35_419 = ROcp35_46*C19+S19*S5;
+    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
+    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
+    ROcp35_719 = -ROcp35_46*S19+C19*S5;
+    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
+    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
+    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
+    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
+    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
+    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
+    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
+    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
+    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
+    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
+    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
+    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
+    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
+    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
+    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
+    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
+    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
+    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
+    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
+    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
+    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
+    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
+    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
+    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
+    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
+    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
+    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
+    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
+    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
+    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
+    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
+    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
+    OMcp35_25 = qd[5]*C4;
+    OMcp35_35 = qd[5]*S4;
+    OMcp35_16 = qd[4]+qd[6]*S5;
+    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
+    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
+    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
+    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
+    POcp35_17 = q[1]+RLcp35_17;
+    POcp35_27 = q[2]+RLcp35_27;
+    POcp35_37 = q[3]+RLcp35_37;
+    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
+    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
+    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
+    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
+    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
+    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
+    VIcp35_17 = qd[1]+ORcp35_17;
+    VIcp35_27 = qd[2]+ORcp35_27;
+    VIcp35_37 = qd[3]+ORcp35_37;
+    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
+    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
+    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
+    RLcp35_19 = ROcp35_720*dpt[3][36];
+    RLcp35_29 = ROcp35_820*dpt[3][36];
+    RLcp35_39 = ROcp35_920*dpt[3][36];
+    POcp35_19 = POcp35_17+RLcp35_19;
+    POcp35_29 = POcp35_27+RLcp35_29;
+    POcp35_39 = POcp35_37+RLcp35_39;
+    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
+    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
+    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
+    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
+    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
+    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
+    VIcp35_19 = ORcp35_19+VIcp35_17;
+    VIcp35_29 = ORcp35_29+VIcp35_27;
+    VIcp35_39 = ORcp35_39+VIcp35_37;
+    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
+    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
+    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
+    POcp35_110 = POcp35_19+RLcp35_110;
+    POcp35_210 = POcp35_29+RLcp35_210;
+    POcp35_310 = POcp35_39+RLcp35_310;
+    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
+    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
+    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
+    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
+    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
+    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
+    VIcp35_110 = ORcp35_110+VIcp35_19;
+    VIcp35_210 = ORcp35_210+VIcp35_29;
+    VIcp35_310 = ORcp35_310+VIcp35_39;
+    RLcp35_111 = ROcp35_421*dpt[2][52];
+    RLcp35_211 = ROcp35_521*dpt[2][52];
+    RLcp35_311 = ROcp35_621*dpt[2][52];
+    POcp35_111 = POcp35_110+RLcp35_111;
+    POcp35_211 = POcp35_210+RLcp35_211;
+    POcp35_311 = POcp35_310+RLcp35_311;
+    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
+    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
+    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
+    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
+    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
+    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
+    VIcp35_111 = ORcp35_111+VIcp35_110;
+    VIcp35_211 = ORcp35_211+VIcp35_210;
+    VIcp35_311 = ORcp35_311+VIcp35_310;
+    RLcp35_112 = ROcp35_727*dpt[3][54];
+    RLcp35_212 = ROcp35_827*dpt[3][54];
+    RLcp35_312 = ROcp35_927*dpt[3][54];
+    POcp35_112 = POcp35_111+RLcp35_112;
+    POcp35_212 = POcp35_211+RLcp35_212;
+    POcp35_312 = POcp35_311+RLcp35_312;
+    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
+    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
+    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
+    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
+    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
+    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
+    VIcp35_112 = ORcp35_112+VIcp35_111;
+    VIcp35_212 = ORcp35_212+VIcp35_211;
+    VIcp35_312 = ORcp35_312+VIcp35_311;
+    PxF11[1] = POcp35_112;
+    PxF11[2] = POcp35_212;
+    PxF11[3] = POcp35_312;
+    RxF11[1][1] = ROcp35_128;
+    RxF11[1][2] = ROcp35_228;
+    RxF11[1][3] = ROcp35_328;
+    RxF11[2][1] = ROcp35_428;
+    RxF11[2][2] = ROcp35_528;
+    RxF11[2][3] = ROcp35_628;
+    RxF11[3][1] = ROcp35_727;
+    RxF11[3][2] = ROcp35_827;
+    RxF11[3][3] = ROcp35_927;
+    VxF11[1] = VIcp35_112;
+    VxF11[2] = VIcp35_212;
+    VxF11[3] = VIcp35_312;
+    OMxF11[1] = OMcp35_112;
+    OMxF11[2] = OMcp35_212;
+    OMxF11[3] = OMcp35_312;
+    ROcp36_25 = S4*S5;
+    ROcp36_35 = -C4*S5;
+    ROcp36_85 = -S4*C5;
+    ROcp36_95 = C4*C5;
+    ROcp36_16 = C5*C6;
+    ROcp36_26 = ROcp36_25*C6+C4*S6;
+    ROcp36_36 = ROcp36_35*C6+S4*S6;
+    ROcp36_46 = -C5*S6;
+    ROcp36_56 = -ROcp36_25*S6+C4*C6;
+    ROcp36_66 = -ROcp36_35*S6+S4*C6;
+    ROcp36_419 = ROcp36_46*C19+S19*S5;
+    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
+    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
+    ROcp36_719 = -ROcp36_46*S19+C19*S5;
+    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
+    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
+    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
+    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
+    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
+    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
+    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
+    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
+    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
+    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
+    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
+    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
+    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
+    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
+    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
+    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
+    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
+    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
+    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
+    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
+    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
+    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
+    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
+    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
+    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
+    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
+    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
+    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
+    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
+    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
+    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
+    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
+    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
+    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
+    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
+    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
+    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
+    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
+    OMcp36_25 = qd[5]*C4;
+    OMcp36_35 = qd[5]*S4;
+    OMcp36_16 = qd[4]+qd[6]*S5;
+    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
+    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
+    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
+    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
+    POcp36_17 = q[1]+RLcp36_17;
+    POcp36_27 = q[2]+RLcp36_27;
+    POcp36_37 = q[3]+RLcp36_37;
+    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
+    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
+    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
+    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
+    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
+    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
+    VIcp36_17 = qd[1]+ORcp36_17;
+    VIcp36_27 = qd[2]+ORcp36_27;
+    VIcp36_37 = qd[3]+ORcp36_37;
+    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
+    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
+    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
+    RLcp36_19 = ROcp36_720*dpt[3][36];
+    RLcp36_29 = ROcp36_820*dpt[3][36];
+    RLcp36_39 = ROcp36_920*dpt[3][36];
+    POcp36_19 = POcp36_17+RLcp36_19;
+    POcp36_29 = POcp36_27+RLcp36_29;
+    POcp36_39 = POcp36_37+RLcp36_39;
+    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
+    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
+    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
+    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
+    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
+    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
+    VIcp36_19 = ORcp36_19+VIcp36_17;
+    VIcp36_29 = ORcp36_29+VIcp36_27;
+    VIcp36_39 = ORcp36_39+VIcp36_37;
+    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
+    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
+    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
+    POcp36_110 = POcp36_19+RLcp36_110;
+    POcp36_210 = POcp36_29+RLcp36_210;
+    POcp36_310 = POcp36_39+RLcp36_310;
+    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
+    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
+    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
+    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
+    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
+    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
+    VIcp36_110 = ORcp36_110+VIcp36_19;
+    VIcp36_210 = ORcp36_210+VIcp36_29;
+    VIcp36_310 = ORcp36_310+VIcp36_39;
+    RLcp36_111 = ROcp36_421*dpt[2][52];
+    RLcp36_211 = ROcp36_521*dpt[2][52];
+    RLcp36_311 = ROcp36_621*dpt[2][52];
+    POcp36_111 = POcp36_110+RLcp36_111;
+    POcp36_211 = POcp36_210+RLcp36_211;
+    POcp36_311 = POcp36_310+RLcp36_311;
+    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
+    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
+    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
+    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
+    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
+    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
+    VIcp36_111 = ORcp36_111+VIcp36_110;
+    VIcp36_211 = ORcp36_211+VIcp36_210;
+    VIcp36_311 = ORcp36_311+VIcp36_310;
+    RLcp36_112 = ROcp36_727*dpt[3][54];
+    RLcp36_212 = ROcp36_827*dpt[3][54];
+    RLcp36_312 = ROcp36_927*dpt[3][54];
+    POcp36_112 = POcp36_111+RLcp36_112;
+    POcp36_212 = POcp36_211+RLcp36_212;
+    POcp36_312 = POcp36_311+RLcp36_312;
+    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
+    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
+    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
+    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
+    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
+    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
+    VIcp36_112 = ORcp36_112+VIcp36_111;
+    VIcp36_212 = ORcp36_212+VIcp36_211;
+    VIcp36_312 = ORcp36_312+VIcp36_311;
+    RLcp36_113 = ROcp36_727*dpt[3][57];
+    RLcp36_213 = ROcp36_827*dpt[3][57];
+    RLcp36_313 = ROcp36_927*dpt[3][57];
+    POcp36_113 = POcp36_112+RLcp36_113;
+    POcp36_213 = POcp36_212+RLcp36_213;
+    POcp36_313 = POcp36_312+RLcp36_313;
+    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
+    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
+    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
+    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
+    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
+    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
+    VIcp36_113 = ORcp36_113+VIcp36_112;
+    VIcp36_213 = ORcp36_213+VIcp36_212;
+    VIcp36_313 = ORcp36_313+VIcp36_312;
+    PxF12[1] = POcp36_113;
+    PxF12[2] = POcp36_213;
+    PxF12[3] = POcp36_313;
+    RxF12[1][1] = ROcp36_129;
+    RxF12[1][2] = ROcp36_229;
+    RxF12[1][3] = ROcp36_329;
+    RxF12[2][1] = ROcp36_428;
+    RxF12[2][2] = ROcp36_528;
+    RxF12[2][3] = ROcp36_628;
+    RxF12[3][1] = ROcp36_729;
+    RxF12[3][2] = ROcp36_829;
+    RxF12[3][3] = ROcp36_929;
+    VxF12[1] = VIcp36_113;
+    VxF12[2] = VIcp36_213;
+    VxF12[3] = VIcp36_313;
+    OMxF12[1] = OMcp36_113;
+    OMxF12[2] = OMcp36_213;
+    OMxF12[3] = OMcp36_313;
+ 
+    // output copy
+    for(i=1; i<=3; i++)
+    {
+        PxF_tab[0][i] = PxF1[i];
+        PxF_tab[1][i] = PxF2[i];
+        PxF_tab[2][i] = PxF3[i];
+        PxF_tab[3][i] = PxF4[i];
+        PxF_tab[4][i] = PxF5[i];
+        PxF_tab[5][i] = PxF6[i];
+        PxF_tab[6][i] = PxF7[i];
+        PxF_tab[7][i] = PxF8[i];
+        PxF_tab[8][i] = PxF9[i];
+        PxF_tab[9][i] = PxF10[i];
+        PxF_tab[10][i] = PxF11[i];
+        PxF_tab[11][i] = PxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        for(j=1; j<=3; j++)
+        {
+        RxF_tab[0][i][j] = RxF1[i][j];
+        RxF_tab[1][i][j] = RxF2[i][j];
+        RxF_tab[2][i][j] = RxF3[i][j];
+        RxF_tab[3][i][j] = RxF4[i][j];
+        RxF_tab[4][i][j] = RxF5[i][j];
+        RxF_tab[5][i][j] = RxF6[i][j];
+        RxF_tab[6][i][j] = RxF7[i][j];
+        RxF_tab[7][i][j] = RxF8[i][j];
+        RxF_tab[8][i][j] = RxF9[i][j];
+        RxF_tab[9][i][j] = RxF10[i][j];
+        RxF_tab[10][i][j] = RxF11[i][j];
+        RxF_tab[11][i][j] = RxF12[i][j];
+        }
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        VxF_tab[0][i] = VxF1[i];
+        VxF_tab[1][i] = VxF2[i];
+        VxF_tab[2][i] = VxF3[i];
+        VxF_tab[3][i] = VxF4[i];
+        VxF_tab[4][i] = VxF5[i];
+        VxF_tab[5][i] = VxF6[i];
+        VxF_tab[6][i] = VxF7[i];
+        VxF_tab[7][i] = VxF8[i];
+        VxF_tab[8][i] = VxF9[i];
+        VxF_tab[9][i] = VxF10[i];
+        VxF_tab[10][i] = VxF11[i];
+        VxF_tab[11][i] = VxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        OMxF_tab[0][i] = OMxF1[i];
+        OMxF_tab[1][i] = OMxF2[i];
+        OMxF_tab[2][i] = OMxF3[i];
+        OMxF_tab[3][i] = OMxF4[i];
+        OMxF_tab[4][i] = OMxF5[i];
+        OMxF_tab[5][i] = OMxF6[i];
+        OMxF_tab[6][i] = OMxF7[i];
+        OMxF_tab[7][i] = OMxF8[i];
+        OMxF_tab[8][i] = OMxF9[i];
+        OMxF_tab[9][i] = OMxF10[i];
+        OMxF_tab[10][i] = OMxF11[i];
+        OMxF_tab[11][i] = OMxF12[i];
+    }
+
+}
diff --git a/dataR/extra/long_feet/specific_simu_index.cc b/dataR/extra/long_feet/specific_simu_index.cc
new file mode 100644
index 0000000..40fc295
--- /dev/null
+++ b/dataR/extra/long_feet/specific_simu_index.cc
@@ -0,0 +1,234 @@
+#include "ModelSimuIndex.hh"
+#include "SimuIndex.hh"
+#include "user_all_id.h"
+
+#include <iostream>
+#include <stdlib.h>
+
+/*! \brief get the indexes related to the MBS file for the joints
+ * 
+ * \param[in] joint_id ID of the requested joint
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsJointId(int joint_id)
+{
+	switch (joint_id)
+	{
+		// floating base
+		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
+		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
+		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
+		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
+		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
+		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
+
+		// right leg
+		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
+		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
+		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
+		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
+		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
+		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
+
+		// left leg
+		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
+		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
+		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
+		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
+		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
+		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
+
+		// torso
+		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
+		case SimuJointIndex::TorsoPitch : return WaistSag_id;
+		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
+
+		// right arm
+		case SimuJointIndex::RightShPitch  : return RShSag_id;
+		case SimuJointIndex::RightShRoll   : return RShLat_id;
+		case SimuJointIndex::RightShYaw    : return RShYaw_id;
+		case SimuJointIndex::RightElbPitch : return RElb_id;
+
+		// left arm
+		case SimuJointIndex::LeftShPitch  : return LShSag_id;
+		case SimuJointIndex::LeftShRoll   : return LShLat_id;
+		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
+		case SimuJointIndex::LeftElbPitch : return LElb_id;
+	
+		default:
+			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the bodies
+ * 
+ * \param[in] body_id ID of the requested body
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsBodyId(int body_id)
+{
+	switch (body_id)
+	{
+		case SimuBodyIndex::Waist : return Waist_id;
+
+		case SimuBodyIndex::RHipmot : return RHipmot_id;
+		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
+		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
+		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
+		case SimuBodyIndex::RFootmot : return RFootmot_id;
+		case SimuBodyIndex::RFoot    : return RFoot_id;
+
+		case SimuBodyIndex::LHipmot : return LHipmot_id;
+		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
+		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
+		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
+		case SimuBodyIndex::LFootmot : return LFootmot_id;
+		case SimuBodyIndex::LFoot    : return LFoot_id;
+
+		case SimuBodyIndex::DWL : return DWL_id;
+		case SimuBodyIndex::DWS : return DWS_id;
+		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
+
+		case SimuBodyIndex::RShp : return RShp_id;
+		case SimuBodyIndex::RShr : return RShr_id;
+		case SimuBodyIndex::RShy : return RShy_id;
+		case SimuBodyIndex::RElb : return RElb_id;
+		case SimuBodyIndex::LShp : return LShp_id;
+		case SimuBodyIndex::LShr : return LShr_id;
+		case SimuBodyIndex::LShy : return LShy_id;
+		case SimuBodyIndex::LElb : return LElb_id;
+	
+		default:
+				std::cout << "Error: unknown body index: " << body_id << std::endl;
+				exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the S sensors
+ * 
+ * \param[in] Ssens_id ID of the requested S sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
+{
+	switch (Ssens_id)
+	{
+		// mid-waist
+		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
+
+		// one part feet
+		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
+		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
+
+		// legs
+		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
+		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
+		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
+		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
+
+		// torso
+		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
+
+		// arms
+		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
+		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
+		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
+		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
+
+		// com
+		case SimuSsensIndex::WaistCg : return WaistCg_id;
+
+		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
+		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
+		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
+
+		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
+		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
+		case SimuSsensIndex::RFootCg    : return RFootCg_id;
+
+		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
+		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
+		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
+
+		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
+		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
+		case SimuSsensIndex::LFootCg    : return LFootCg_id;
+
+		case SimuSsensIndex::DWLCg : return DWLCg_id;
+		case SimuSsensIndex::DWSCg : return DWSCg_id;
+		case SimuSsensIndex::DWYCg : return DWYCg_id;
+
+		case SimuSsensIndex::RShpCg : return RShpCg_id;
+		case SimuSsensIndex::RShrCg : return RShrCg_id;
+		case SimuSsensIndex::RShyCg : return RShyCg_id;
+		case SimuSsensIndex::RElbCg : return RElbCg_id;
+
+		case SimuSsensIndex::LShpCg : return LShpCg_id;
+		case SimuSsensIndex::LShrCg : return LShrCg_id;
+		case SimuSsensIndex::LShyCg : return LShyCg_id;
+		case SimuSsensIndex::LElbCg : return LElbCg_id;
+	
+		default:
+			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the F sensors
+ * 
+ * \param[in] Fsens_id ID of the requested F sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
+{
+	switch (Fsens_id)
+	{
+		// mid-waist
+		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
+
+		// legs
+		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
+		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
+		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
+		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
+
+		// one part feet
+		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
+		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
+
+		// torso
+		case SimuFsensIndex::Torso : return Torso_contact_force_id;
+
+		// arms
+		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
+		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
+		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
+		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
+	
+		default:
+			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the links
+ * 
+ * \param[in] link_id ID of the requested link
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsLinkId(int link_id)
+{
+	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
+	exit(EXIT_FAILURE);
+
+	return -1;
+}
diff --git a/dataR/extra/long_feet/user_kinematics_model.c b/dataR/extra/long_feet/user_kinematics_model.c
new file mode 100644
index 0000000..542f5ca
--- /dev/null
+++ b/dataR/extra/long_feet/user_kinematics_model.c
@@ -0,0 +1,1734 @@
+#include "mbs_data.h"
+
+#include "math.h"
+
+/*! \brief compute the kinematics model of sensors with ExtForces
+ * 
+ * \param[in] mbs_data Robotran structure
+ * \param[out] PxF_tab tabular with all the 'PxF' computed
+ * \param[out] RxF_tab tabular with all the 'RxF' computed
+ * \param[out] VxF_tab tabular with all the 'VxF' computed
+ * \param[out] OMxF_tab tabular with all the 'OMxF' computed
+ */
+void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
+{
+    // variables declaration
+    int i, j;
+    double *q, *qd;
+    double **dpt;
+
+    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
+    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
+    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
+    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
+
+    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
+    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
+
+    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
+    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
+    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
+    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
+    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
+    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
+    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
+    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
+    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
+    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
+    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
+    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
+    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
+    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
+    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
+    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
+    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
+    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
+    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
+    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
+    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
+    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
+    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
+    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
+    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
+    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
+    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
+    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
+    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
+    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
+    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
+    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
+    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
+    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
+    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
+    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
+    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
+    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
+    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
+    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
+    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
+    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
+    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
+    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
+    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
+    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
+    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
+    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
+    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
+    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
+    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
+    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
+    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
+    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
+    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
+    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
+    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
+    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
+    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
+    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
+    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
+    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
+    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
+    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
+    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
+    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
+    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
+    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
+    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
+    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
+    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
+    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
+    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
+    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
+    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
+    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
+    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
+    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
+    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
+    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
+    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
+    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
+    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
+    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
+    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
+
+    // variables initialization
+    q   = mbs_data->q;
+    qd  = mbs_data->qd;
+    dpt = mbs_data->dpt;
+
+    // cosines and sines
+    C4 = cos(q[4]);
+    C5 = cos(q[5]);
+    C6 = cos(q[6]);
+    C7 = cos(q[7]);
+    C8 = cos(q[8]);
+    C9 = cos(q[9]);
+    C10 = cos(q[10]);
+    C11 = cos(q[11]);
+    C12 = cos(q[12]);
+    C13 = cos(q[13]);
+    C14 = cos(q[14]);
+    C15 = cos(q[15]);
+    C16 = cos(q[16]);
+    C17 = cos(q[17]);
+    C18 = cos(q[18]);
+    C19 = cos(q[19]);
+    C20 = cos(q[20]);
+    C21 = cos(q[21]);
+    C22 = cos(q[22]);
+    C23 = cos(q[23]);
+    C24 = cos(q[24]);
+    C25 = cos(q[25]);
+    C26 = cos(q[26]);
+    C27 = cos(q[27]);
+    C28 = cos(q[28]);
+    C29 = cos(q[29]);
+    S4 = sin(q[4]);
+    S5 = sin(q[5]);
+    S6 = sin(q[6]);
+    S7 = sin(q[7]);
+    S8 = sin(q[8]);
+    S9 = sin(q[9]);
+    S10 = sin(q[10]);
+    S11 = sin(q[11]);
+    S12 = sin(q[12]);
+    S13 = sin(q[13]);
+    S14 = sin(q[14]);
+    S15 = sin(q[15]);
+    S16 = sin(q[16]);
+    S17 = sin(q[17]);
+    S18 = sin(q[18]);
+    S19 = sin(q[19]);
+    S20 = sin(q[20]);
+    S21 = sin(q[21]);
+    S22 = sin(q[22]);
+    S23 = sin(q[23]);
+    S24 = sin(q[24]);
+    S25 = sin(q[25]);
+    S26 = sin(q[26]);
+    S27 = sin(q[27]);
+    S28 = sin(q[28]);
+    S29 = sin(q[29]);
+
+    // main computation
+    ROcp25_25 = S4*S5;
+    ROcp25_35 = -C4*S5;
+    ROcp25_85 = -S4*C5;
+    ROcp25_95 = C4*C5;
+    ROcp25_16 = C5*C6;
+    ROcp25_26 = ROcp25_25*C6+C4*S6;
+    ROcp25_36 = ROcp25_35*C6+S4*S6;
+    ROcp25_46 = -C5*S6;
+    ROcp25_56 = -ROcp25_25*S6+C4*C6;
+    ROcp25_66 = -ROcp25_35*S6+S4*C6;
+    OMcp25_25 = qd[5]*C4;
+    OMcp25_35 = qd[5]*S4;
+    OMcp25_16 = qd[4]+qd[6]*S5;
+    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
+    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
+    PxF1[1] = q[1];
+    PxF1[2] = q[2];
+    PxF1[3] = q[3];
+    RxF1[1][1] = ROcp25_16;
+    RxF1[1][2] = ROcp25_26;
+    RxF1[1][3] = ROcp25_36;
+    RxF1[2][1] = ROcp25_46;
+    RxF1[2][2] = ROcp25_56;
+    RxF1[2][3] = ROcp25_66;
+    RxF1[3][1] = S5;
+    RxF1[3][2] = ROcp25_85;
+    RxF1[3][3] = ROcp25_95;
+    VxF1[1] = qd[1];
+    VxF1[2] = qd[2];
+    VxF1[3] = qd[3];
+    OMxF1[1] = OMcp25_16;
+    OMxF1[2] = OMcp25_26;
+    OMxF1[3] = OMcp25_36;
+    ROcp26_25 = S4*S5;
+    ROcp26_35 = -C4*S5;
+    ROcp26_85 = -S4*C5;
+    ROcp26_95 = C4*C5;
+    ROcp26_16 = C5*C6;
+    ROcp26_26 = ROcp26_25*C6+C4*S6;
+    ROcp26_36 = ROcp26_35*C6+S4*S6;
+    ROcp26_46 = -C5*S6;
+    ROcp26_56 = -ROcp26_25*S6+C4*C6;
+    ROcp26_66 = -ROcp26_35*S6+S4*C6;
+    ROcp26_17 = ROcp26_16*C7-S5*S7;
+    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
+    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
+    ROcp26_77 = ROcp26_16*S7+S5*C7;
+    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
+    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
+    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
+    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
+    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
+    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
+    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
+    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
+    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
+    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
+    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
+    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
+    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
+    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
+    OMcp26_25 = qd[5]*C4;
+    OMcp26_35 = qd[5]*S4;
+    OMcp26_16 = qd[4]+qd[6]*S5;
+    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
+    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
+    RLcp26_17 = ROcp26_46*dpt[2][2];
+    RLcp26_27 = ROcp26_56*dpt[2][2];
+    RLcp26_37 = ROcp26_66*dpt[2][2];
+    POcp26_17 = q[1]+RLcp26_17;
+    POcp26_27 = q[2]+RLcp26_27;
+    POcp26_37 = q[3]+RLcp26_37;
+    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
+    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
+    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
+    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
+    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
+    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
+    VIcp26_17 = qd[1]+ORcp26_17;
+    VIcp26_27 = qd[2]+ORcp26_27;
+    VIcp26_37 = qd[3]+ORcp26_37;
+    RLcp26_18 = ROcp26_46*dpt[2][6];
+    RLcp26_28 = ROcp26_56*dpt[2][6];
+    RLcp26_38 = ROcp26_66*dpt[2][6];
+    POcp26_18 = POcp26_17+RLcp26_18;
+    POcp26_28 = POcp26_27+RLcp26_28;
+    POcp26_38 = POcp26_37+RLcp26_38;
+    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
+    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
+    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
+    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
+    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
+    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
+    VIcp26_18 = ORcp26_18+VIcp26_17;
+    VIcp26_28 = ORcp26_28+VIcp26_27;
+    VIcp26_38 = ORcp26_38+VIcp26_37;
+    RLcp26_19 = ROcp26_78*dpt[3][8];
+    RLcp26_29 = ROcp26_88*dpt[3][8];
+    RLcp26_39 = ROcp26_98*dpt[3][8];
+    POcp26_19 = POcp26_18+RLcp26_19;
+    POcp26_29 = POcp26_28+RLcp26_29;
+    POcp26_39 = POcp26_38+RLcp26_39;
+    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
+    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
+    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
+    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
+    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
+    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
+    VIcp26_19 = ORcp26_19+VIcp26_18;
+    VIcp26_29 = ORcp26_29+VIcp26_28;
+    VIcp26_39 = ORcp26_39+VIcp26_38;
+    PxF2[1] = POcp26_19;
+    PxF2[2] = POcp26_29;
+    PxF2[3] = POcp26_39;
+    RxF2[1][1] = ROcp26_19;
+    RxF2[1][2] = ROcp26_29;
+    RxF2[1][3] = ROcp26_39;
+    RxF2[2][1] = ROcp26_49;
+    RxF2[2][2] = ROcp26_59;
+    RxF2[2][3] = ROcp26_69;
+    RxF2[3][1] = ROcp26_78;
+    RxF2[3][2] = ROcp26_88;
+    RxF2[3][3] = ROcp26_98;
+    VxF2[1] = VIcp26_19;
+    VxF2[2] = VIcp26_29;
+    VxF2[3] = VIcp26_39;
+    OMxF2[1] = OMcp26_19;
+    OMxF2[2] = OMcp26_29;
+    OMxF2[3] = OMcp26_39;
+    ROcp27_25 = S4*S5;
+    ROcp27_35 = -C4*S5;
+    ROcp27_85 = -S4*C5;
+    ROcp27_95 = C4*C5;
+    ROcp27_16 = C5*C6;
+    ROcp27_26 = ROcp27_25*C6+C4*S6;
+    ROcp27_36 = ROcp27_35*C6+S4*S6;
+    ROcp27_46 = -C5*S6;
+    ROcp27_56 = -ROcp27_25*S6+C4*C6;
+    ROcp27_66 = -ROcp27_35*S6+S4*C6;
+    ROcp27_17 = ROcp27_16*C7-S5*S7;
+    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
+    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
+    ROcp27_77 = ROcp27_16*S7+S5*C7;
+    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
+    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
+    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
+    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
+    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
+    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
+    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
+    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
+    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
+    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
+    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
+    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
+    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
+    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
+    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
+    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
+    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
+    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
+    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
+    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
+    OMcp27_25 = qd[5]*C4;
+    OMcp27_35 = qd[5]*S4;
+    OMcp27_16 = qd[4]+qd[6]*S5;
+    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
+    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
+    RLcp27_17 = ROcp27_46*dpt[2][2];
+    RLcp27_27 = ROcp27_56*dpt[2][2];
+    RLcp27_37 = ROcp27_66*dpt[2][2];
+    POcp27_17 = q[1]+RLcp27_17;
+    POcp27_27 = q[2]+RLcp27_27;
+    POcp27_37 = q[3]+RLcp27_37;
+    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
+    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
+    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
+    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
+    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
+    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
+    VIcp27_17 = qd[1]+ORcp27_17;
+    VIcp27_27 = qd[2]+ORcp27_27;
+    VIcp27_37 = qd[3]+ORcp27_37;
+    RLcp27_18 = ROcp27_46*dpt[2][6];
+    RLcp27_28 = ROcp27_56*dpt[2][6];
+    RLcp27_38 = ROcp27_66*dpt[2][6];
+    POcp27_18 = POcp27_17+RLcp27_18;
+    POcp27_28 = POcp27_27+RLcp27_28;
+    POcp27_38 = POcp27_37+RLcp27_38;
+    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
+    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
+    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
+    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
+    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
+    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
+    VIcp27_18 = ORcp27_18+VIcp27_17;
+    VIcp27_28 = ORcp27_28+VIcp27_27;
+    VIcp27_38 = ORcp27_38+VIcp27_37;
+    RLcp27_19 = ROcp27_78*dpt[3][8];
+    RLcp27_29 = ROcp27_88*dpt[3][8];
+    RLcp27_39 = ROcp27_98*dpt[3][8];
+    POcp27_19 = POcp27_18+RLcp27_19;
+    POcp27_29 = POcp27_28+RLcp27_29;
+    POcp27_39 = POcp27_38+RLcp27_39;
+    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
+    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
+    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
+    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
+    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
+    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
+    VIcp27_19 = ORcp27_19+VIcp27_18;
+    VIcp27_29 = ORcp27_29+VIcp27_28;
+    VIcp27_39 = ORcp27_39+VIcp27_38;
+    RLcp27_110 = ROcp27_78*dpt[3][11];
+    RLcp27_210 = ROcp27_88*dpt[3][11];
+    RLcp27_310 = ROcp27_98*dpt[3][11];
+    POcp27_110 = POcp27_19+RLcp27_110;
+    POcp27_210 = POcp27_29+RLcp27_210;
+    POcp27_310 = POcp27_39+RLcp27_310;
+    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
+    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
+    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
+    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
+    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
+    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
+    VIcp27_110 = ORcp27_110+VIcp27_19;
+    VIcp27_210 = ORcp27_210+VIcp27_29;
+    VIcp27_310 = ORcp27_310+VIcp27_39;
+    PxF3[1] = POcp27_110;
+    PxF3[2] = POcp27_210;
+    PxF3[3] = POcp27_310;
+    RxF3[1][1] = ROcp27_110;
+    RxF3[1][2] = ROcp27_210;
+    RxF3[1][3] = ROcp27_310;
+    RxF3[2][1] = ROcp27_49;
+    RxF3[2][2] = ROcp27_59;
+    RxF3[2][3] = ROcp27_69;
+    RxF3[3][1] = ROcp27_710;
+    RxF3[3][2] = ROcp27_810;
+    RxF3[3][3] = ROcp27_910;
+    VxF3[1] = VIcp27_110;
+    VxF3[2] = VIcp27_210;
+    VxF3[3] = VIcp27_310;
+    OMxF3[1] = OMcp27_110;
+    OMxF3[2] = OMcp27_210;
+    OMxF3[3] = OMcp27_310;
+    ROcp28_25 = S4*S5;
+    ROcp28_35 = -C4*S5;
+    ROcp28_85 = -S4*C5;
+    ROcp28_95 = C4*C5;
+    ROcp28_16 = C5*C6;
+    ROcp28_26 = ROcp28_25*C6+C4*S6;
+    ROcp28_36 = ROcp28_35*C6+S4*S6;
+    ROcp28_46 = -C5*S6;
+    ROcp28_56 = -ROcp28_25*S6+C4*C6;
+    ROcp28_66 = -ROcp28_35*S6+S4*C6;
+    ROcp28_17 = ROcp28_16*C7-S5*S7;
+    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
+    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
+    ROcp28_77 = ROcp28_16*S7+S5*C7;
+    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
+    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
+    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
+    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
+    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
+    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
+    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
+    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
+    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
+    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
+    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
+    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
+    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
+    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
+    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
+    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
+    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
+    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
+    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
+    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
+    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
+    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
+    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
+    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
+    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
+    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
+    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
+    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
+    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
+    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
+    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
+    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
+    OMcp28_25 = qd[5]*C4;
+    OMcp28_35 = qd[5]*S4;
+    OMcp28_16 = qd[4]+qd[6]*S5;
+    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
+    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
+    RLcp28_17 = ROcp28_46*dpt[2][2];
+    RLcp28_27 = ROcp28_56*dpt[2][2];
+    RLcp28_37 = ROcp28_66*dpt[2][2];
+    POcp28_17 = q[1]+RLcp28_17;
+    POcp28_27 = q[2]+RLcp28_27;
+    POcp28_37 = q[3]+RLcp28_37;
+    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
+    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
+    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
+    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
+    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
+    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
+    VIcp28_17 = qd[1]+ORcp28_17;
+    VIcp28_27 = qd[2]+ORcp28_27;
+    VIcp28_37 = qd[3]+ORcp28_37;
+    RLcp28_18 = ROcp28_46*dpt[2][6];
+    RLcp28_28 = ROcp28_56*dpt[2][6];
+    RLcp28_38 = ROcp28_66*dpt[2][6];
+    POcp28_18 = POcp28_17+RLcp28_18;
+    POcp28_28 = POcp28_27+RLcp28_28;
+    POcp28_38 = POcp28_37+RLcp28_38;
+    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
+    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
+    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
+    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
+    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
+    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
+    VIcp28_18 = ORcp28_18+VIcp28_17;
+    VIcp28_28 = ORcp28_28+VIcp28_27;
+    VIcp28_38 = ORcp28_38+VIcp28_37;
+    RLcp28_19 = ROcp28_78*dpt[3][8];
+    RLcp28_29 = ROcp28_88*dpt[3][8];
+    RLcp28_39 = ROcp28_98*dpt[3][8];
+    POcp28_19 = POcp28_18+RLcp28_19;
+    POcp28_29 = POcp28_28+RLcp28_29;
+    POcp28_39 = POcp28_38+RLcp28_39;
+    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
+    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
+    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
+    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
+    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
+    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
+    VIcp28_19 = ORcp28_19+VIcp28_18;
+    VIcp28_29 = ORcp28_29+VIcp28_28;
+    VIcp28_39 = ORcp28_39+VIcp28_38;
+    RLcp28_110 = ROcp28_78*dpt[3][11];
+    RLcp28_210 = ROcp28_88*dpt[3][11];
+    RLcp28_310 = ROcp28_98*dpt[3][11];
+    POcp28_110 = POcp28_19+RLcp28_110;
+    POcp28_210 = POcp28_29+RLcp28_210;
+    POcp28_310 = POcp28_39+RLcp28_310;
+    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
+    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
+    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
+    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
+    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
+    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
+    VIcp28_110 = ORcp28_110+VIcp28_19;
+    VIcp28_210 = ORcp28_210+VIcp28_29;
+    VIcp28_310 = ORcp28_310+VIcp28_39;
+    RLcp28_111 = ROcp28_710*dpt[3][14];
+    RLcp28_211 = ROcp28_810*dpt[3][14];
+    RLcp28_311 = ROcp28_910*dpt[3][14];
+    POcp28_111 = POcp28_110+RLcp28_111;
+    POcp28_211 = POcp28_210+RLcp28_211;
+    POcp28_311 = POcp28_310+RLcp28_311;
+    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
+    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
+    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
+    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
+    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
+    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
+    VIcp28_111 = ORcp28_111+VIcp28_110;
+    VIcp28_211 = ORcp28_211+VIcp28_210;
+    VIcp28_311 = ORcp28_311+VIcp28_310;
+    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
+    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
+    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
+    RLcp28_113 = ROcp28_712*dpt[3][18];
+    RLcp28_213 = ROcp28_812*dpt[3][18];
+    RLcp28_313 = ROcp28_912*dpt[3][18];
+    POcp28_113 = POcp28_111+RLcp28_113;
+    POcp28_213 = POcp28_211+RLcp28_213;
+    POcp28_313 = POcp28_311+RLcp28_313;
+    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
+    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
+    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
+    VIcp28_113 = ORcp28_113+VIcp28_111;
+    VIcp28_213 = ORcp28_213+VIcp28_211;
+    VIcp28_313 = ORcp28_313+VIcp28_311;
+    PxF4[1] = POcp28_113;
+    PxF4[2] = POcp28_213;
+    PxF4[3] = POcp28_313;
+    RxF4[1][1] = ROcp28_112;
+    RxF4[1][2] = ROcp28_212;
+    RxF4[1][3] = ROcp28_312;
+    RxF4[2][1] = ROcp28_411;
+    RxF4[2][2] = ROcp28_511;
+    RxF4[2][3] = ROcp28_611;
+    RxF4[3][1] = ROcp28_712;
+    RxF4[3][2] = ROcp28_812;
+    RxF4[3][3] = ROcp28_912;
+    VxF4[1] = VIcp28_113;
+    VxF4[2] = VIcp28_213;
+    VxF4[3] = VIcp28_313;
+    OMxF4[1] = OMcp28_112;
+    OMxF4[2] = OMcp28_212;
+    OMxF4[3] = OMcp28_312;
+    ROcp29_25 = S4*S5;
+    ROcp29_35 = -C4*S5;
+    ROcp29_85 = -S4*C5;
+    ROcp29_95 = C4*C5;
+    ROcp29_16 = C5*C6;
+    ROcp29_26 = ROcp29_25*C6+C4*S6;
+    ROcp29_36 = ROcp29_35*C6+S4*S6;
+    ROcp29_46 = -C5*S6;
+    ROcp29_56 = -ROcp29_25*S6+C4*C6;
+    ROcp29_66 = -ROcp29_35*S6+S4*C6;
+    ROcp29_113 = ROcp29_16*C13-S13*S5;
+    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
+    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
+    ROcp29_713 = ROcp29_16*S13+C13*S5;
+    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
+    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
+    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
+    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
+    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
+    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
+    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
+    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
+    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
+    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
+    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
+    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
+    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
+    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
+    OMcp29_25 = qd[5]*C4;
+    OMcp29_35 = qd[5]*S4;
+    OMcp29_16 = qd[4]+qd[6]*S5;
+    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
+    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
+    RLcp29_17 = ROcp29_46*dpt[2][3];
+    RLcp29_27 = ROcp29_56*dpt[2][3];
+    RLcp29_37 = ROcp29_66*dpt[2][3];
+    POcp29_17 = q[1]+RLcp29_17;
+    POcp29_27 = q[2]+RLcp29_27;
+    POcp29_37 = q[3]+RLcp29_37;
+    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
+    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
+    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
+    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
+    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
+    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
+    VIcp29_17 = qd[1]+ORcp29_17;
+    VIcp29_27 = qd[2]+ORcp29_27;
+    VIcp29_37 = qd[3]+ORcp29_37;
+    RLcp29_18 = ROcp29_46*dpt[2][20];
+    RLcp29_28 = ROcp29_56*dpt[2][20];
+    RLcp29_38 = ROcp29_66*dpt[2][20];
+    POcp29_18 = POcp29_17+RLcp29_18;
+    POcp29_28 = POcp29_27+RLcp29_28;
+    POcp29_38 = POcp29_37+RLcp29_38;
+    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
+    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
+    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
+    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
+    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
+    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
+    VIcp29_18 = ORcp29_18+VIcp29_17;
+    VIcp29_28 = ORcp29_28+VIcp29_27;
+    VIcp29_38 = ORcp29_38+VIcp29_37;
+    RLcp29_19 = ROcp29_714*dpt[3][22];
+    RLcp29_29 = ROcp29_814*dpt[3][22];
+    RLcp29_39 = ROcp29_914*dpt[3][22];
+    POcp29_19 = POcp29_18+RLcp29_19;
+    POcp29_29 = POcp29_28+RLcp29_29;
+    POcp29_39 = POcp29_38+RLcp29_39;
+    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
+    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
+    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
+    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
+    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
+    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
+    VIcp29_19 = ORcp29_19+VIcp29_18;
+    VIcp29_29 = ORcp29_29+VIcp29_28;
+    VIcp29_39 = ORcp29_39+VIcp29_38;
+    PxF5[1] = POcp29_19;
+    PxF5[2] = POcp29_29;
+    PxF5[3] = POcp29_39;
+    RxF5[1][1] = ROcp29_115;
+    RxF5[1][2] = ROcp29_215;
+    RxF5[1][3] = ROcp29_315;
+    RxF5[2][1] = ROcp29_415;
+    RxF5[2][2] = ROcp29_515;
+    RxF5[2][3] = ROcp29_615;
+    RxF5[3][1] = ROcp29_714;
+    RxF5[3][2] = ROcp29_814;
+    RxF5[3][3] = ROcp29_914;
+    VxF5[1] = VIcp29_19;
+    VxF5[2] = VIcp29_29;
+    VxF5[3] = VIcp29_39;
+    OMxF5[1] = OMcp29_19;
+    OMxF5[2] = OMcp29_29;
+    OMxF5[3] = OMcp29_39;
+    ROcp30_25 = S4*S5;
+    ROcp30_35 = -C4*S5;
+    ROcp30_85 = -S4*C5;
+    ROcp30_95 = C4*C5;
+    ROcp30_16 = C5*C6;
+    ROcp30_26 = ROcp30_25*C6+C4*S6;
+    ROcp30_36 = ROcp30_35*C6+S4*S6;
+    ROcp30_46 = -C5*S6;
+    ROcp30_56 = -ROcp30_25*S6+C4*C6;
+    ROcp30_66 = -ROcp30_35*S6+S4*C6;
+    ROcp30_113 = ROcp30_16*C13-S13*S5;
+    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
+    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
+    ROcp30_713 = ROcp30_16*S13+C13*S5;
+    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
+    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
+    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
+    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
+    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
+    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
+    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
+    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
+    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
+    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
+    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
+    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
+    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
+    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
+    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
+    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
+    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
+    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
+    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
+    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
+    OMcp30_25 = qd[5]*C4;
+    OMcp30_35 = qd[5]*S4;
+    OMcp30_16 = qd[4]+qd[6]*S5;
+    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
+    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
+    RLcp30_17 = ROcp30_46*dpt[2][3];
+    RLcp30_27 = ROcp30_56*dpt[2][3];
+    RLcp30_37 = ROcp30_66*dpt[2][3];
+    POcp30_17 = q[1]+RLcp30_17;
+    POcp30_27 = q[2]+RLcp30_27;
+    POcp30_37 = q[3]+RLcp30_37;
+    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
+    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
+    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
+    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
+    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
+    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
+    VIcp30_17 = qd[1]+ORcp30_17;
+    VIcp30_27 = qd[2]+ORcp30_27;
+    VIcp30_37 = qd[3]+ORcp30_37;
+    RLcp30_18 = ROcp30_46*dpt[2][20];
+    RLcp30_28 = ROcp30_56*dpt[2][20];
+    RLcp30_38 = ROcp30_66*dpt[2][20];
+    POcp30_18 = POcp30_17+RLcp30_18;
+    POcp30_28 = POcp30_27+RLcp30_28;
+    POcp30_38 = POcp30_37+RLcp30_38;
+    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
+    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
+    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
+    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
+    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
+    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
+    VIcp30_18 = ORcp30_18+VIcp30_17;
+    VIcp30_28 = ORcp30_28+VIcp30_27;
+    VIcp30_38 = ORcp30_38+VIcp30_37;
+    RLcp30_19 = ROcp30_714*dpt[3][22];
+    RLcp30_29 = ROcp30_814*dpt[3][22];
+    RLcp30_39 = ROcp30_914*dpt[3][22];
+    POcp30_19 = POcp30_18+RLcp30_19;
+    POcp30_29 = POcp30_28+RLcp30_29;
+    POcp30_39 = POcp30_38+RLcp30_39;
+    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
+    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
+    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
+    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
+    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
+    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
+    VIcp30_19 = ORcp30_19+VIcp30_18;
+    VIcp30_29 = ORcp30_29+VIcp30_28;
+    VIcp30_39 = ORcp30_39+VIcp30_38;
+    RLcp30_110 = ROcp30_714*dpt[3][25];
+    RLcp30_210 = ROcp30_814*dpt[3][25];
+    RLcp30_310 = ROcp30_914*dpt[3][25];
+    POcp30_110 = POcp30_19+RLcp30_110;
+    POcp30_210 = POcp30_29+RLcp30_210;
+    POcp30_310 = POcp30_39+RLcp30_310;
+    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
+    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
+    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
+    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
+    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
+    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
+    VIcp30_110 = ORcp30_110+VIcp30_19;
+    VIcp30_210 = ORcp30_210+VIcp30_29;
+    VIcp30_310 = ORcp30_310+VIcp30_39;
+    PxF6[1] = POcp30_110;
+    PxF6[2] = POcp30_210;
+    PxF6[3] = POcp30_310;
+    RxF6[1][1] = ROcp30_116;
+    RxF6[1][2] = ROcp30_216;
+    RxF6[1][3] = ROcp30_316;
+    RxF6[2][1] = ROcp30_415;
+    RxF6[2][2] = ROcp30_515;
+    RxF6[2][3] = ROcp30_615;
+    RxF6[3][1] = ROcp30_716;
+    RxF6[3][2] = ROcp30_816;
+    RxF6[3][3] = ROcp30_916;
+    VxF6[1] = VIcp30_110;
+    VxF6[2] = VIcp30_210;
+    VxF6[3] = VIcp30_310;
+    OMxF6[1] = OMcp30_110;
+    OMxF6[2] = OMcp30_210;
+    OMxF6[3] = OMcp30_310;
+    ROcp31_25 = S4*S5;
+    ROcp31_35 = -C4*S5;
+    ROcp31_85 = -S4*C5;
+    ROcp31_95 = C4*C5;
+    ROcp31_16 = C5*C6;
+    ROcp31_26 = ROcp31_25*C6+C4*S6;
+    ROcp31_36 = ROcp31_35*C6+S4*S6;
+    ROcp31_46 = -C5*S6;
+    ROcp31_56 = -ROcp31_25*S6+C4*C6;
+    ROcp31_66 = -ROcp31_35*S6+S4*C6;
+    ROcp31_113 = ROcp31_16*C13-S13*S5;
+    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
+    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
+    ROcp31_713 = ROcp31_16*S13+C13*S5;
+    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
+    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
+    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
+    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
+    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
+    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
+    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
+    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
+    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
+    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
+    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
+    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
+    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
+    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
+    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
+    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
+    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
+    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
+    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
+    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
+    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
+    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
+    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
+    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
+    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
+    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
+    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
+    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
+    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
+    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
+    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
+    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
+    OMcp31_25 = qd[5]*C4;
+    OMcp31_35 = qd[5]*S4;
+    OMcp31_16 = qd[4]+qd[6]*S5;
+    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
+    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
+    RLcp31_17 = ROcp31_46*dpt[2][3];
+    RLcp31_27 = ROcp31_56*dpt[2][3];
+    RLcp31_37 = ROcp31_66*dpt[2][3];
+    POcp31_17 = q[1]+RLcp31_17;
+    POcp31_27 = q[2]+RLcp31_27;
+    POcp31_37 = q[3]+RLcp31_37;
+    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
+    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
+    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
+    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
+    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
+    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
+    VIcp31_17 = qd[1]+ORcp31_17;
+    VIcp31_27 = qd[2]+ORcp31_27;
+    VIcp31_37 = qd[3]+ORcp31_37;
+    RLcp31_18 = ROcp31_46*dpt[2][20];
+    RLcp31_28 = ROcp31_56*dpt[2][20];
+    RLcp31_38 = ROcp31_66*dpt[2][20];
+    POcp31_18 = POcp31_17+RLcp31_18;
+    POcp31_28 = POcp31_27+RLcp31_28;
+    POcp31_38 = POcp31_37+RLcp31_38;
+    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
+    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
+    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
+    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
+    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
+    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
+    VIcp31_18 = ORcp31_18+VIcp31_17;
+    VIcp31_28 = ORcp31_28+VIcp31_27;
+    VIcp31_38 = ORcp31_38+VIcp31_37;
+    RLcp31_19 = ROcp31_714*dpt[3][22];
+    RLcp31_29 = ROcp31_814*dpt[3][22];
+    RLcp31_39 = ROcp31_914*dpt[3][22];
+    POcp31_19 = POcp31_18+RLcp31_19;
+    POcp31_29 = POcp31_28+RLcp31_29;
+    POcp31_39 = POcp31_38+RLcp31_39;
+    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
+    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
+    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
+    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
+    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
+    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
+    VIcp31_19 = ORcp31_19+VIcp31_18;
+    VIcp31_29 = ORcp31_29+VIcp31_28;
+    VIcp31_39 = ORcp31_39+VIcp31_38;
+    RLcp31_110 = ROcp31_714*dpt[3][25];
+    RLcp31_210 = ROcp31_814*dpt[3][25];
+    RLcp31_310 = ROcp31_914*dpt[3][25];
+    POcp31_110 = POcp31_19+RLcp31_110;
+    POcp31_210 = POcp31_29+RLcp31_210;
+    POcp31_310 = POcp31_39+RLcp31_310;
+    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
+    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
+    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
+    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
+    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
+    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
+    VIcp31_110 = ORcp31_110+VIcp31_19;
+    VIcp31_210 = ORcp31_210+VIcp31_29;
+    VIcp31_310 = ORcp31_310+VIcp31_39;
+    RLcp31_111 = ROcp31_716*dpt[3][28];
+    RLcp31_211 = ROcp31_816*dpt[3][28];
+    RLcp31_311 = ROcp31_916*dpt[3][28];
+    POcp31_111 = POcp31_110+RLcp31_111;
+    POcp31_211 = POcp31_210+RLcp31_211;
+    POcp31_311 = POcp31_310+RLcp31_311;
+    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
+    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
+    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
+    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
+    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
+    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
+    VIcp31_111 = ORcp31_111+VIcp31_110;
+    VIcp31_211 = ORcp31_211+VIcp31_210;
+    VIcp31_311 = ORcp31_311+VIcp31_310;
+    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
+    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
+    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
+    RLcp31_113 = ROcp31_718*dpt[3][33];
+    RLcp31_213 = ROcp31_818*dpt[3][33];
+    RLcp31_313 = ROcp31_918*dpt[3][33];
+    POcp31_113 = POcp31_111+RLcp31_113;
+    POcp31_213 = POcp31_211+RLcp31_213;
+    POcp31_313 = POcp31_311+RLcp31_313;
+    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
+    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
+    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
+    VIcp31_113 = ORcp31_113+VIcp31_111;
+    VIcp31_213 = ORcp31_213+VIcp31_211;
+    VIcp31_313 = ORcp31_313+VIcp31_311;
+    PxF7[1] = POcp31_113;
+    PxF7[2] = POcp31_213;
+    PxF7[3] = POcp31_313;
+    RxF7[1][1] = ROcp31_118;
+    RxF7[1][2] = ROcp31_218;
+    RxF7[1][3] = ROcp31_318;
+    RxF7[2][1] = ROcp31_417;
+    RxF7[2][2] = ROcp31_517;
+    RxF7[2][3] = ROcp31_617;
+    RxF7[3][1] = ROcp31_718;
+    RxF7[3][2] = ROcp31_818;
+    RxF7[3][3] = ROcp31_918;
+    VxF7[1] = VIcp31_113;
+    VxF7[2] = VIcp31_213;
+    VxF7[3] = VIcp31_313;
+    OMxF7[1] = OMcp31_112;
+    OMxF7[2] = OMcp31_212;
+    OMxF7[3] = OMcp31_312;
+    ROcp32_25 = S4*S5;
+    ROcp32_35 = -C4*S5;
+    ROcp32_85 = -S4*C5;
+    ROcp32_95 = C4*C5;
+    ROcp32_16 = C5*C6;
+    ROcp32_26 = ROcp32_25*C6+C4*S6;
+    ROcp32_36 = ROcp32_35*C6+S4*S6;
+    ROcp32_46 = -C5*S6;
+    ROcp32_56 = -ROcp32_25*S6+C4*C6;
+    ROcp32_66 = -ROcp32_35*S6+S4*C6;
+    ROcp32_419 = ROcp32_46*C19+S19*S5;
+    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
+    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
+    ROcp32_719 = -ROcp32_46*S19+C19*S5;
+    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
+    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
+    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
+    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
+    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
+    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
+    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
+    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
+    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
+    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
+    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
+    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
+    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
+    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
+    OMcp32_25 = qd[5]*C4;
+    OMcp32_35 = qd[5]*S4;
+    OMcp32_16 = qd[4]+qd[6]*S5;
+    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
+    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
+    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
+    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
+    POcp32_17 = q[1]+RLcp32_17;
+    POcp32_27 = q[2]+RLcp32_27;
+    POcp32_37 = q[3]+RLcp32_37;
+    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
+    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
+    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
+    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
+    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
+    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
+    VIcp32_17 = qd[1]+ORcp32_17;
+    VIcp32_27 = qd[2]+ORcp32_27;
+    VIcp32_37 = qd[3]+ORcp32_37;
+    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
+    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
+    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
+    RLcp32_19 = ROcp32_720*dpt[3][36];
+    RLcp32_29 = ROcp32_820*dpt[3][36];
+    RLcp32_39 = ROcp32_920*dpt[3][36];
+    POcp32_19 = POcp32_17+RLcp32_19;
+    POcp32_29 = POcp32_27+RLcp32_29;
+    POcp32_39 = POcp32_37+RLcp32_39;
+    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
+    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
+    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
+    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
+    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
+    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
+    VIcp32_19 = ORcp32_19+VIcp32_17;
+    VIcp32_29 = ORcp32_29+VIcp32_27;
+    VIcp32_39 = ORcp32_39+VIcp32_37;
+    PxF8[1] = POcp32_19;
+    PxF8[2] = POcp32_29;
+    PxF8[3] = POcp32_39;
+    RxF8[1][1] = ROcp32_121;
+    RxF8[1][2] = ROcp32_221;
+    RxF8[1][3] = ROcp32_321;
+    RxF8[2][1] = ROcp32_421;
+    RxF8[2][2] = ROcp32_521;
+    RxF8[2][3] = ROcp32_621;
+    RxF8[3][1] = ROcp32_720;
+    RxF8[3][2] = ROcp32_820;
+    RxF8[3][3] = ROcp32_920;
+    VxF8[1] = VIcp32_19;
+    VxF8[2] = VIcp32_29;
+    VxF8[3] = VIcp32_39;
+    OMxF8[1] = OMcp32_19;
+    OMxF8[2] = OMcp32_29;
+    OMxF8[3] = OMcp32_39;
+    ROcp33_25 = S4*S5;
+    ROcp33_35 = -C4*S5;
+    ROcp33_85 = -S4*C5;
+    ROcp33_95 = C4*C5;
+    ROcp33_16 = C5*C6;
+    ROcp33_26 = ROcp33_25*C6+C4*S6;
+    ROcp33_36 = ROcp33_35*C6+S4*S6;
+    ROcp33_46 = -C5*S6;
+    ROcp33_56 = -ROcp33_25*S6+C4*C6;
+    ROcp33_66 = -ROcp33_35*S6+S4*C6;
+    ROcp33_419 = ROcp33_46*C19+S19*S5;
+    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
+    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
+    ROcp33_719 = -ROcp33_46*S19+C19*S5;
+    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
+    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
+    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
+    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
+    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
+    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
+    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
+    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
+    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
+    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
+    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
+    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
+    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
+    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
+    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
+    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
+    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
+    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
+    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
+    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
+    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
+    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
+    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
+    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
+    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
+    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
+    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
+    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
+    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
+    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
+    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
+    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
+    OMcp33_25 = qd[5]*C4;
+    OMcp33_35 = qd[5]*S4;
+    OMcp33_16 = qd[4]+qd[6]*S5;
+    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
+    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
+    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
+    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
+    POcp33_17 = q[1]+RLcp33_17;
+    POcp33_27 = q[2]+RLcp33_27;
+    POcp33_37 = q[3]+RLcp33_37;
+    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
+    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
+    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
+    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
+    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
+    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
+    VIcp33_17 = qd[1]+ORcp33_17;
+    VIcp33_27 = qd[2]+ORcp33_27;
+    VIcp33_37 = qd[3]+ORcp33_37;
+    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
+    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
+    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
+    RLcp33_19 = ROcp33_720*dpt[3][36];
+    RLcp33_29 = ROcp33_820*dpt[3][36];
+    RLcp33_39 = ROcp33_920*dpt[3][36];
+    POcp33_19 = POcp33_17+RLcp33_19;
+    POcp33_29 = POcp33_27+RLcp33_29;
+    POcp33_39 = POcp33_37+RLcp33_39;
+    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
+    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
+    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
+    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
+    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
+    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
+    VIcp33_19 = ORcp33_19+VIcp33_17;
+    VIcp33_29 = ORcp33_29+VIcp33_27;
+    VIcp33_39 = ORcp33_39+VIcp33_37;
+    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
+    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
+    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
+    POcp33_110 = POcp33_19+RLcp33_110;
+    POcp33_210 = POcp33_29+RLcp33_210;
+    POcp33_310 = POcp33_39+RLcp33_310;
+    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
+    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
+    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
+    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
+    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
+    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
+    VIcp33_110 = ORcp33_110+VIcp33_19;
+    VIcp33_210 = ORcp33_210+VIcp33_29;
+    VIcp33_310 = ORcp33_310+VIcp33_39;
+    RLcp33_111 = ROcp33_421*dpt[2][43];
+    RLcp33_211 = ROcp33_521*dpt[2][43];
+    RLcp33_311 = ROcp33_621*dpt[2][43];
+    POcp33_111 = POcp33_110+RLcp33_111;
+    POcp33_211 = POcp33_210+RLcp33_211;
+    POcp33_311 = POcp33_310+RLcp33_311;
+    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
+    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
+    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
+    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
+    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
+    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
+    VIcp33_111 = ORcp33_111+VIcp33_110;
+    VIcp33_211 = ORcp33_211+VIcp33_210;
+    VIcp33_311 = ORcp33_311+VIcp33_310;
+    RLcp33_112 = ROcp33_723*dpt[3][45];
+    RLcp33_212 = ROcp33_823*dpt[3][45];
+    RLcp33_312 = ROcp33_923*dpt[3][45];
+    POcp33_112 = POcp33_111+RLcp33_112;
+    POcp33_212 = POcp33_211+RLcp33_212;
+    POcp33_312 = POcp33_311+RLcp33_312;
+    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
+    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
+    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
+    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
+    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
+    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
+    VIcp33_112 = ORcp33_112+VIcp33_111;
+    VIcp33_212 = ORcp33_212+VIcp33_211;
+    VIcp33_312 = ORcp33_312+VIcp33_311;
+    PxF9[1] = POcp33_112;
+    PxF9[2] = POcp33_212;
+    PxF9[3] = POcp33_312;
+    RxF9[1][1] = ROcp33_124;
+    RxF9[1][2] = ROcp33_224;
+    RxF9[1][3] = ROcp33_324;
+    RxF9[2][1] = ROcp33_424;
+    RxF9[2][2] = ROcp33_524;
+    RxF9[2][3] = ROcp33_624;
+    RxF9[3][1] = ROcp33_723;
+    RxF9[3][2] = ROcp33_823;
+    RxF9[3][3] = ROcp33_923;
+    VxF9[1] = VIcp33_112;
+    VxF9[2] = VIcp33_212;
+    VxF9[3] = VIcp33_312;
+    OMxF9[1] = OMcp33_112;
+    OMxF9[2] = OMcp33_212;
+    OMxF9[3] = OMcp33_312;
+    ROcp34_25 = S4*S5;
+    ROcp34_35 = -C4*S5;
+    ROcp34_85 = -S4*C5;
+    ROcp34_95 = C4*C5;
+    ROcp34_16 = C5*C6;
+    ROcp34_26 = ROcp34_25*C6+C4*S6;
+    ROcp34_36 = ROcp34_35*C6+S4*S6;
+    ROcp34_46 = -C5*S6;
+    ROcp34_56 = -ROcp34_25*S6+C4*C6;
+    ROcp34_66 = -ROcp34_35*S6+S4*C6;
+    ROcp34_419 = ROcp34_46*C19+S19*S5;
+    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
+    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
+    ROcp34_719 = -ROcp34_46*S19+C19*S5;
+    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
+    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
+    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
+    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
+    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
+    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
+    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
+    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
+    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
+    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
+    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
+    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
+    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
+    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
+    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
+    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
+    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
+    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
+    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
+    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
+    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
+    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
+    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
+    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
+    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
+    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
+    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
+    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
+    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
+    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
+    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
+    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
+    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
+    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
+    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
+    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
+    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
+    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
+    OMcp34_25 = qd[5]*C4;
+    OMcp34_35 = qd[5]*S4;
+    OMcp34_16 = qd[4]+qd[6]*S5;
+    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
+    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
+    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
+    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
+    POcp34_17 = q[1]+RLcp34_17;
+    POcp34_27 = q[2]+RLcp34_27;
+    POcp34_37 = q[3]+RLcp34_37;
+    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
+    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
+    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
+    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
+    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
+    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
+    VIcp34_17 = qd[1]+ORcp34_17;
+    VIcp34_27 = qd[2]+ORcp34_27;
+    VIcp34_37 = qd[3]+ORcp34_37;
+    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
+    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
+    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
+    RLcp34_19 = ROcp34_720*dpt[3][36];
+    RLcp34_29 = ROcp34_820*dpt[3][36];
+    RLcp34_39 = ROcp34_920*dpt[3][36];
+    POcp34_19 = POcp34_17+RLcp34_19;
+    POcp34_29 = POcp34_27+RLcp34_29;
+    POcp34_39 = POcp34_37+RLcp34_39;
+    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
+    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
+    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
+    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
+    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
+    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
+    VIcp34_19 = ORcp34_19+VIcp34_17;
+    VIcp34_29 = ORcp34_29+VIcp34_27;
+    VIcp34_39 = ORcp34_39+VIcp34_37;
+    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
+    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
+    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
+    POcp34_110 = POcp34_19+RLcp34_110;
+    POcp34_210 = POcp34_29+RLcp34_210;
+    POcp34_310 = POcp34_39+RLcp34_310;
+    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
+    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
+    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
+    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
+    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
+    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
+    VIcp34_110 = ORcp34_110+VIcp34_19;
+    VIcp34_210 = ORcp34_210+VIcp34_29;
+    VIcp34_310 = ORcp34_310+VIcp34_39;
+    RLcp34_111 = ROcp34_421*dpt[2][43];
+    RLcp34_211 = ROcp34_521*dpt[2][43];
+    RLcp34_311 = ROcp34_621*dpt[2][43];
+    POcp34_111 = POcp34_110+RLcp34_111;
+    POcp34_211 = POcp34_210+RLcp34_211;
+    POcp34_311 = POcp34_310+RLcp34_311;
+    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
+    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
+    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
+    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
+    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
+    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
+    VIcp34_111 = ORcp34_111+VIcp34_110;
+    VIcp34_211 = ORcp34_211+VIcp34_210;
+    VIcp34_311 = ORcp34_311+VIcp34_310;
+    RLcp34_112 = ROcp34_723*dpt[3][45];
+    RLcp34_212 = ROcp34_823*dpt[3][45];
+    RLcp34_312 = ROcp34_923*dpt[3][45];
+    POcp34_112 = POcp34_111+RLcp34_112;
+    POcp34_212 = POcp34_211+RLcp34_212;
+    POcp34_312 = POcp34_311+RLcp34_312;
+    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
+    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
+    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
+    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
+    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
+    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
+    VIcp34_112 = ORcp34_112+VIcp34_111;
+    VIcp34_212 = ORcp34_212+VIcp34_211;
+    VIcp34_312 = ORcp34_312+VIcp34_311;
+    RLcp34_113 = ROcp34_723*dpt[3][48];
+    RLcp34_213 = ROcp34_823*dpt[3][48];
+    RLcp34_313 = ROcp34_923*dpt[3][48];
+    POcp34_113 = POcp34_112+RLcp34_113;
+    POcp34_213 = POcp34_212+RLcp34_213;
+    POcp34_313 = POcp34_312+RLcp34_313;
+    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
+    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
+    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
+    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
+    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
+    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
+    VIcp34_113 = ORcp34_113+VIcp34_112;
+    VIcp34_213 = ORcp34_213+VIcp34_212;
+    VIcp34_313 = ORcp34_313+VIcp34_312;
+    PxF10[1] = POcp34_113;
+    PxF10[2] = POcp34_213;
+    PxF10[3] = POcp34_313;
+    RxF10[1][1] = ROcp34_125;
+    RxF10[1][2] = ROcp34_225;
+    RxF10[1][3] = ROcp34_325;
+    RxF10[2][1] = ROcp34_424;
+    RxF10[2][2] = ROcp34_524;
+    RxF10[2][3] = ROcp34_624;
+    RxF10[3][1] = ROcp34_725;
+    RxF10[3][2] = ROcp34_825;
+    RxF10[3][3] = ROcp34_925;
+    VxF10[1] = VIcp34_113;
+    VxF10[2] = VIcp34_213;
+    VxF10[3] = VIcp34_313;
+    OMxF10[1] = OMcp34_113;
+    OMxF10[2] = OMcp34_213;
+    OMxF10[3] = OMcp34_313;
+    ROcp35_25 = S4*S5;
+    ROcp35_35 = -C4*S5;
+    ROcp35_85 = -S4*C5;
+    ROcp35_95 = C4*C5;
+    ROcp35_16 = C5*C6;
+    ROcp35_26 = ROcp35_25*C6+C4*S6;
+    ROcp35_36 = ROcp35_35*C6+S4*S6;
+    ROcp35_46 = -C5*S6;
+    ROcp35_56 = -ROcp35_25*S6+C4*C6;
+    ROcp35_66 = -ROcp35_35*S6+S4*C6;
+    ROcp35_419 = ROcp35_46*C19+S19*S5;
+    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
+    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
+    ROcp35_719 = -ROcp35_46*S19+C19*S5;
+    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
+    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
+    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
+    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
+    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
+    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
+    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
+    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
+    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
+    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
+    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
+    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
+    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
+    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
+    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
+    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
+    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
+    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
+    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
+    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
+    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
+    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
+    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
+    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
+    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
+    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
+    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
+    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
+    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
+    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
+    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
+    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
+    OMcp35_25 = qd[5]*C4;
+    OMcp35_35 = qd[5]*S4;
+    OMcp35_16 = qd[4]+qd[6]*S5;
+    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
+    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
+    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
+    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
+    POcp35_17 = q[1]+RLcp35_17;
+    POcp35_27 = q[2]+RLcp35_27;
+    POcp35_37 = q[3]+RLcp35_37;
+    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
+    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
+    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
+    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
+    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
+    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
+    VIcp35_17 = qd[1]+ORcp35_17;
+    VIcp35_27 = qd[2]+ORcp35_27;
+    VIcp35_37 = qd[3]+ORcp35_37;
+    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
+    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
+    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
+    RLcp35_19 = ROcp35_720*dpt[3][36];
+    RLcp35_29 = ROcp35_820*dpt[3][36];
+    RLcp35_39 = ROcp35_920*dpt[3][36];
+    POcp35_19 = POcp35_17+RLcp35_19;
+    POcp35_29 = POcp35_27+RLcp35_29;
+    POcp35_39 = POcp35_37+RLcp35_39;
+    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
+    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
+    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
+    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
+    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
+    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
+    VIcp35_19 = ORcp35_19+VIcp35_17;
+    VIcp35_29 = ORcp35_29+VIcp35_27;
+    VIcp35_39 = ORcp35_39+VIcp35_37;
+    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
+    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
+    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
+    POcp35_110 = POcp35_19+RLcp35_110;
+    POcp35_210 = POcp35_29+RLcp35_210;
+    POcp35_310 = POcp35_39+RLcp35_310;
+    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
+    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
+    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
+    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
+    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
+    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
+    VIcp35_110 = ORcp35_110+VIcp35_19;
+    VIcp35_210 = ORcp35_210+VIcp35_29;
+    VIcp35_310 = ORcp35_310+VIcp35_39;
+    RLcp35_111 = ROcp35_421*dpt[2][52];
+    RLcp35_211 = ROcp35_521*dpt[2][52];
+    RLcp35_311 = ROcp35_621*dpt[2][52];
+    POcp35_111 = POcp35_110+RLcp35_111;
+    POcp35_211 = POcp35_210+RLcp35_211;
+    POcp35_311 = POcp35_310+RLcp35_311;
+    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
+    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
+    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
+    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
+    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
+    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
+    VIcp35_111 = ORcp35_111+VIcp35_110;
+    VIcp35_211 = ORcp35_211+VIcp35_210;
+    VIcp35_311 = ORcp35_311+VIcp35_310;
+    RLcp35_112 = ROcp35_727*dpt[3][54];
+    RLcp35_212 = ROcp35_827*dpt[3][54];
+    RLcp35_312 = ROcp35_927*dpt[3][54];
+    POcp35_112 = POcp35_111+RLcp35_112;
+    POcp35_212 = POcp35_211+RLcp35_212;
+    POcp35_312 = POcp35_311+RLcp35_312;
+    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
+    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
+    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
+    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
+    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
+    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
+    VIcp35_112 = ORcp35_112+VIcp35_111;
+    VIcp35_212 = ORcp35_212+VIcp35_211;
+    VIcp35_312 = ORcp35_312+VIcp35_311;
+    PxF11[1] = POcp35_112;
+    PxF11[2] = POcp35_212;
+    PxF11[3] = POcp35_312;
+    RxF11[1][1] = ROcp35_128;
+    RxF11[1][2] = ROcp35_228;
+    RxF11[1][3] = ROcp35_328;
+    RxF11[2][1] = ROcp35_428;
+    RxF11[2][2] = ROcp35_528;
+    RxF11[2][3] = ROcp35_628;
+    RxF11[3][1] = ROcp35_727;
+    RxF11[3][2] = ROcp35_827;
+    RxF11[3][3] = ROcp35_927;
+    VxF11[1] = VIcp35_112;
+    VxF11[2] = VIcp35_212;
+    VxF11[3] = VIcp35_312;
+    OMxF11[1] = OMcp35_112;
+    OMxF11[2] = OMcp35_212;
+    OMxF11[3] = OMcp35_312;
+    ROcp36_25 = S4*S5;
+    ROcp36_35 = -C4*S5;
+    ROcp36_85 = -S4*C5;
+    ROcp36_95 = C4*C5;
+    ROcp36_16 = C5*C6;
+    ROcp36_26 = ROcp36_25*C6+C4*S6;
+    ROcp36_36 = ROcp36_35*C6+S4*S6;
+    ROcp36_46 = -C5*S6;
+    ROcp36_56 = -ROcp36_25*S6+C4*C6;
+    ROcp36_66 = -ROcp36_35*S6+S4*C6;
+    ROcp36_419 = ROcp36_46*C19+S19*S5;
+    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
+    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
+    ROcp36_719 = -ROcp36_46*S19+C19*S5;
+    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
+    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
+    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
+    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
+    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
+    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
+    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
+    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
+    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
+    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
+    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
+    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
+    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
+    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
+    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
+    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
+    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
+    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
+    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
+    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
+    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
+    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
+    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
+    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
+    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
+    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
+    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
+    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
+    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
+    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
+    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
+    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
+    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
+    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
+    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
+    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
+    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
+    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
+    OMcp36_25 = qd[5]*C4;
+    OMcp36_35 = qd[5]*S4;
+    OMcp36_16 = qd[4]+qd[6]*S5;
+    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
+    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
+    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
+    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
+    POcp36_17 = q[1]+RLcp36_17;
+    POcp36_27 = q[2]+RLcp36_27;
+    POcp36_37 = q[3]+RLcp36_37;
+    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
+    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
+    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
+    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
+    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
+    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
+    VIcp36_17 = qd[1]+ORcp36_17;
+    VIcp36_27 = qd[2]+ORcp36_27;
+    VIcp36_37 = qd[3]+ORcp36_37;
+    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
+    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
+    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
+    RLcp36_19 = ROcp36_720*dpt[3][36];
+    RLcp36_29 = ROcp36_820*dpt[3][36];
+    RLcp36_39 = ROcp36_920*dpt[3][36];
+    POcp36_19 = POcp36_17+RLcp36_19;
+    POcp36_29 = POcp36_27+RLcp36_29;
+    POcp36_39 = POcp36_37+RLcp36_39;
+    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
+    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
+    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
+    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
+    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
+    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
+    VIcp36_19 = ORcp36_19+VIcp36_17;
+    VIcp36_29 = ORcp36_29+VIcp36_27;
+    VIcp36_39 = ORcp36_39+VIcp36_37;
+    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
+    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
+    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
+    POcp36_110 = POcp36_19+RLcp36_110;
+    POcp36_210 = POcp36_29+RLcp36_210;
+    POcp36_310 = POcp36_39+RLcp36_310;
+    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
+    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
+    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
+    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
+    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
+    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
+    VIcp36_110 = ORcp36_110+VIcp36_19;
+    VIcp36_210 = ORcp36_210+VIcp36_29;
+    VIcp36_310 = ORcp36_310+VIcp36_39;
+    RLcp36_111 = ROcp36_421*dpt[2][52];
+    RLcp36_211 = ROcp36_521*dpt[2][52];
+    RLcp36_311 = ROcp36_621*dpt[2][52];
+    POcp36_111 = POcp36_110+RLcp36_111;
+    POcp36_211 = POcp36_210+RLcp36_211;
+    POcp36_311 = POcp36_310+RLcp36_311;
+    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
+    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
+    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
+    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
+    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
+    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
+    VIcp36_111 = ORcp36_111+VIcp36_110;
+    VIcp36_211 = ORcp36_211+VIcp36_210;
+    VIcp36_311 = ORcp36_311+VIcp36_310;
+    RLcp36_112 = ROcp36_727*dpt[3][54];
+    RLcp36_212 = ROcp36_827*dpt[3][54];
+    RLcp36_312 = ROcp36_927*dpt[3][54];
+    POcp36_112 = POcp36_111+RLcp36_112;
+    POcp36_212 = POcp36_211+RLcp36_212;
+    POcp36_312 = POcp36_311+RLcp36_312;
+    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
+    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
+    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
+    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
+    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
+    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
+    VIcp36_112 = ORcp36_112+VIcp36_111;
+    VIcp36_212 = ORcp36_212+VIcp36_211;
+    VIcp36_312 = ORcp36_312+VIcp36_311;
+    RLcp36_113 = ROcp36_727*dpt[3][57];
+    RLcp36_213 = ROcp36_827*dpt[3][57];
+    RLcp36_313 = ROcp36_927*dpt[3][57];
+    POcp36_113 = POcp36_112+RLcp36_113;
+    POcp36_213 = POcp36_212+RLcp36_213;
+    POcp36_313 = POcp36_312+RLcp36_313;
+    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
+    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
+    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
+    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
+    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
+    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
+    VIcp36_113 = ORcp36_113+VIcp36_112;
+    VIcp36_213 = ORcp36_213+VIcp36_212;
+    VIcp36_313 = ORcp36_313+VIcp36_312;
+    PxF12[1] = POcp36_113;
+    PxF12[2] = POcp36_213;
+    PxF12[3] = POcp36_313;
+    RxF12[1][1] = ROcp36_129;
+    RxF12[1][2] = ROcp36_229;
+    RxF12[1][3] = ROcp36_329;
+    RxF12[2][1] = ROcp36_428;
+    RxF12[2][2] = ROcp36_528;
+    RxF12[2][3] = ROcp36_628;
+    RxF12[3][1] = ROcp36_729;
+    RxF12[3][2] = ROcp36_829;
+    RxF12[3][3] = ROcp36_929;
+    VxF12[1] = VIcp36_113;
+    VxF12[2] = VIcp36_213;
+    VxF12[3] = VIcp36_313;
+    OMxF12[1] = OMcp36_113;
+    OMxF12[2] = OMcp36_213;
+    OMxF12[3] = OMcp36_313;
+ 
+    // output copy
+    for(i=1; i<=3; i++)
+    {
+        PxF_tab[0][i] = PxF1[i];
+        PxF_tab[1][i] = PxF2[i];
+        PxF_tab[2][i] = PxF3[i];
+        PxF_tab[3][i] = PxF4[i];
+        PxF_tab[4][i] = PxF5[i];
+        PxF_tab[5][i] = PxF6[i];
+        PxF_tab[6][i] = PxF7[i];
+        PxF_tab[7][i] = PxF8[i];
+        PxF_tab[8][i] = PxF9[i];
+        PxF_tab[9][i] = PxF10[i];
+        PxF_tab[10][i] = PxF11[i];
+        PxF_tab[11][i] = PxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        for(j=1; j<=3; j++)
+        {
+        RxF_tab[0][i][j] = RxF1[i][j];
+        RxF_tab[1][i][j] = RxF2[i][j];
+        RxF_tab[2][i][j] = RxF3[i][j];
+        RxF_tab[3][i][j] = RxF4[i][j];
+        RxF_tab[4][i][j] = RxF5[i][j];
+        RxF_tab[5][i][j] = RxF6[i][j];
+        RxF_tab[6][i][j] = RxF7[i][j];
+        RxF_tab[7][i][j] = RxF8[i][j];
+        RxF_tab[8][i][j] = RxF9[i][j];
+        RxF_tab[9][i][j] = RxF10[i][j];
+        RxF_tab[10][i][j] = RxF11[i][j];
+        RxF_tab[11][i][j] = RxF12[i][j];
+        }
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        VxF_tab[0][i] = VxF1[i];
+        VxF_tab[1][i] = VxF2[i];
+        VxF_tab[2][i] = VxF3[i];
+        VxF_tab[3][i] = VxF4[i];
+        VxF_tab[4][i] = VxF5[i];
+        VxF_tab[5][i] = VxF6[i];
+        VxF_tab[6][i] = VxF7[i];
+        VxF_tab[7][i] = VxF8[i];
+        VxF_tab[8][i] = VxF9[i];
+        VxF_tab[9][i] = VxF10[i];
+        VxF_tab[10][i] = VxF11[i];
+        VxF_tab[11][i] = VxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        OMxF_tab[0][i] = OMxF1[i];
+        OMxF_tab[1][i] = OMxF2[i];
+        OMxF_tab[2][i] = OMxF3[i];
+        OMxF_tab[3][i] = OMxF4[i];
+        OMxF_tab[4][i] = OMxF5[i];
+        OMxF_tab[5][i] = OMxF6[i];
+        OMxF_tab[6][i] = OMxF7[i];
+        OMxF_tab[7][i] = OMxF8[i];
+        OMxF_tab[8][i] = OMxF9[i];
+        OMxF_tab[9][i] = OMxF10[i];
+        OMxF_tab[10][i] = OMxF11[i];
+        OMxF_tab[11][i] = OMxF12[i];
+    }
+
+}
diff --git a/dataR/extra/short_feet/specific_simu_index.cc b/dataR/extra/short_feet/specific_simu_index.cc
new file mode 100644
index 0000000..19549c8
--- /dev/null
+++ b/dataR/extra/short_feet/specific_simu_index.cc
@@ -0,0 +1,236 @@
+#include "ModelSimuIndex.hh"
+#include "SimuIndex.hh"
+#include "user_all_id.h"
+
+#include <iostream>
+#include <stdlib.h>
+
+/*! \brief get the indexes related to the MBS file for the joints
+ * 
+ * \param[in] joint_id ID of the requested joint
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsJointId(int joint_id)
+{
+	switch (joint_id)
+	{
+		// floating base
+		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
+		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
+		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
+		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
+		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
+		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
+
+		// right leg
+		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
+		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
+		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
+		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
+		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
+		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
+
+		// left leg
+		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
+		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
+		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
+		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
+		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
+		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
+
+		// torso
+		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
+		case SimuJointIndex::TorsoPitch : return WaistSag_id;
+		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
+
+		// right arm
+		case SimuJointIndex::RightShPitch  : return RShSag_id;
+		case SimuJointIndex::RightShRoll   : return RShLat_id;
+		case SimuJointIndex::RightShYaw    : return RShYaw_id;
+		case SimuJointIndex::RightElbPitch : return RElb_id;
+
+		// left arm
+		case SimuJointIndex::LeftShPitch  : return LShSag_id;
+		case SimuJointIndex::LeftShRoll   : return LShLat_id;
+		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
+		case SimuJointIndex::LeftElbPitch : return LElb_id;
+
+		default:
+			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the bodies
+ * 
+ * \param[in] body_id ID of the requested body
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsBodyId(int body_id)
+{
+	switch (body_id)
+	{
+		case SimuBodyIndex::Waist : return Waist_id;
+
+		case SimuBodyIndex::RHipmot : return RHipmot_id;
+		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
+		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
+		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
+		case SimuBodyIndex::RFootmot : return RFootmot_id;
+		case SimuBodyIndex::RFoot    : return RFoot_id;
+
+		case SimuBodyIndex::LHipmot : return LHipmot_id;
+		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
+		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
+		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
+		case SimuBodyIndex::LFootmot : return LFootmot_id;
+		case SimuBodyIndex::LFoot    : return LFoot_id;
+
+		case SimuBodyIndex::DWL : return DWL_id;
+		case SimuBodyIndex::DWS : return DWS_id;
+		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
+
+		case SimuBodyIndex::RShp : return RShp_id;
+		case SimuBodyIndex::RShr : return RShr_id;
+		case SimuBodyIndex::RShy : return RShy_id;
+		case SimuBodyIndex::RElb : return RElb_id;
+
+		case SimuBodyIndex::LShp : return LShp_id;
+		case SimuBodyIndex::LShr : return LShr_id;
+		case SimuBodyIndex::LShy : return LShy_id;
+		case SimuBodyIndex::LElb : return LElb_id;
+	
+		default:
+				std::cout << "Error: unknown body index: " << body_id << std::endl;
+				exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the S sensors
+ * 
+ * \param[in] Ssens_id ID of the requested S sensor
+ * \param[in] mbs_data Robotran structure
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
+{
+	switch (Ssens_id)
+	{
+		// mid-waist
+		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
+
+		// one part feet
+		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
+		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
+
+		// legs
+		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
+		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
+		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
+		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
+
+		// torso
+		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
+
+		// arms
+		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
+		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
+		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
+		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
+
+		// com
+		case SimuSsensIndex::WaistCg : return WaistCg_id;
+
+		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
+		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
+		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
+
+		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
+		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
+		case SimuSsensIndex::RFootCg    : return RFootCg_id;
+
+		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
+		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
+		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
+
+		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
+		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
+		case SimuSsensIndex::LFootCg    : return LFootCg_id;
+
+		case SimuSsensIndex::DWLCg : return DWLCg_id;
+		case SimuSsensIndex::DWSCg : return DWSCg_id;
+		case SimuSsensIndex::DWYCg : return DWYCg_id;
+
+		case SimuSsensIndex::RShpCg : return RShpCg_id;
+		case SimuSsensIndex::RShrCg : return RShrCg_id;
+		case SimuSsensIndex::RShyCg : return RShyCg_id;
+		case SimuSsensIndex::RElbCg : return RElbCg_id;
+
+		case SimuSsensIndex::LShpCg : return LShpCg_id;
+		case SimuSsensIndex::LShrCg : return LShrCg_id;
+		case SimuSsensIndex::LShyCg : return LShyCg_id;
+		case SimuSsensIndex::LElbCg : return LElbCg_id;
+	
+		default:
+			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the F sensors
+ * 
+ * \param[in] Fsens_id ID of the requested F sensor
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
+{
+	switch (Fsens_id)
+	{
+		// mid-waist
+		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
+
+		// legs
+		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
+		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
+		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
+		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
+
+		// one part feet
+		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
+		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
+
+		// torso
+		case SimuFsensIndex::Torso : return Torso_contact_force_id;
+
+		// arms
+		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
+		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
+		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
+		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
+	
+		default:
+			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
+			exit(EXIT_FAILURE);
+	}
+
+	return -1;
+}
+
+/*! \brief get the indexes related to the MBS file for the links
+ * 
+ * \param[in] link_id ID of the requested link
+ * \return requested MBS index
+ */
+int ModelSimuIndex::ExtractMbsLinkId(int link_id)
+{
+	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
+	exit(EXIT_FAILURE);
+
+	return -1;
+}
diff --git a/dataR/extra/short_feet/user_kinematics_model.c b/dataR/extra/short_feet/user_kinematics_model.c
new file mode 100644
index 0000000..4d78acd
--- /dev/null
+++ b/dataR/extra/short_feet/user_kinematics_model.c
@@ -0,0 +1,1733 @@
+#include "mbs_data.h"
+
+#include "math.h"
+
+/*! \brief compute the kinematics model of sensors with ExtForces
+ * 
+ * \param[in] mbs_data Robotran structure
+ * \param[out] PxF_tab tabular with all the 'PxF' computed
+ * \param[out] RxF_tab tabular with all the 'RxF' computed
+ * \param[out] VxF_tab tabular with all the 'VxF' computed
+ * \param[out] OMxF_tab tabular with all the 'OMxF' computed
+ */
+void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
+{
+    // variables declaration
+    int i, j;
+    double *q, *qd;
+    double **dpt;
+
+    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
+    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
+    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
+    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
+
+    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
+    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
+
+    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
+    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
+    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
+    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
+    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
+    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
+    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
+    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
+    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
+    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
+    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
+    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
+    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
+    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
+    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
+    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
+    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
+    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
+    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
+    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
+    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
+    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
+    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
+    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
+    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
+    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
+    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
+    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
+    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
+    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
+    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
+    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
+    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
+    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
+    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
+    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
+    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
+    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
+    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
+    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
+    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
+    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
+    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
+    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
+    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
+    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
+    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
+    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
+    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
+    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
+    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
+    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
+    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
+    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
+    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
+    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
+    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
+    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
+    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
+    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
+    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
+    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
+    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
+    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
+    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
+    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
+    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
+    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
+    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
+    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
+    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
+    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
+    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
+    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
+    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
+    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
+    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
+    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
+    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
+    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
+    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
+    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
+    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
+    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
+    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
+
+    // variables initialization
+    q   = mbs_data->q;
+    qd  = mbs_data->qd;
+    dpt = mbs_data->dpt;
+
+    // cosines and sines
+    C4 = cos(q[4]);
+    C5 = cos(q[5]);
+    C6 = cos(q[6]);
+    C7 = cos(q[7]);
+    C8 = cos(q[8]);
+    C9 = cos(q[9]);
+    C10 = cos(q[10]);
+    C11 = cos(q[11]);
+    C12 = cos(q[12]);
+    C13 = cos(q[13]);
+    C14 = cos(q[14]);
+    C15 = cos(q[15]);
+    C16 = cos(q[16]);
+    C17 = cos(q[17]);
+    C18 = cos(q[18]);
+    C19 = cos(q[19]);
+    C20 = cos(q[20]);
+    C21 = cos(q[21]);
+    C22 = cos(q[22]);
+    C23 = cos(q[23]);
+    C24 = cos(q[24]);
+    C25 = cos(q[25]);
+    C26 = cos(q[26]);
+    C27 = cos(q[27]);
+    C28 = cos(q[28]);
+    C29 = cos(q[29]);
+    S4 = sin(q[4]);
+    S5 = sin(q[5]);
+    S6 = sin(q[6]);
+    S7 = sin(q[7]);
+    S8 = sin(q[8]);
+    S9 = sin(q[9]);
+    S10 = sin(q[10]);
+    S11 = sin(q[11]);
+    S12 = sin(q[12]);
+    S13 = sin(q[13]);
+    S14 = sin(q[14]);
+    S15 = sin(q[15]);
+    S16 = sin(q[16]);
+    S17 = sin(q[17]);
+    S18 = sin(q[18]);
+    S19 = sin(q[19]);
+    S20 = sin(q[20]);
+    S21 = sin(q[21]);
+    S22 = sin(q[22]);
+    S23 = sin(q[23]);
+    S24 = sin(q[24]);
+    S25 = sin(q[25]);
+    S26 = sin(q[26]);
+    S27 = sin(q[27]);
+    S28 = sin(q[28]);
+    S29 = sin(q[29]);
+
+    // main computation
+    ROcp25_25 = S4*S5;
+    ROcp25_35 = -C4*S5;
+    ROcp25_85 = -S4*C5;
+    ROcp25_95 = C4*C5;
+    ROcp25_16 = C5*C6;
+    ROcp25_26 = ROcp25_25*C6+C4*S6;
+    ROcp25_36 = ROcp25_35*C6+S4*S6;
+    ROcp25_46 = -C5*S6;
+    ROcp25_56 = -ROcp25_25*S6+C4*C6;
+    ROcp25_66 = -ROcp25_35*S6+S4*C6;
+    OMcp25_25 = qd[5]*C4;
+    OMcp25_35 = qd[5]*S4;
+    OMcp25_16 = qd[4]+qd[6]*S5;
+    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
+    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
+    PxF1[1] = q[1];
+    PxF1[2] = q[2];
+    PxF1[3] = q[3];
+    RxF1[1][1] = ROcp25_16;
+    RxF1[1][2] = ROcp25_26;
+    RxF1[1][3] = ROcp25_36;
+    RxF1[2][1] = ROcp25_46;
+    RxF1[2][2] = ROcp25_56;
+    RxF1[2][3] = ROcp25_66;
+    RxF1[3][1] = S5;
+    RxF1[3][2] = ROcp25_85;
+    RxF1[3][3] = ROcp25_95;
+    VxF1[1] = qd[1];
+    VxF1[2] = qd[2];
+    VxF1[3] = qd[3];
+    OMxF1[1] = OMcp25_16;
+    OMxF1[2] = OMcp25_26;
+    OMxF1[3] = OMcp25_36;
+    ROcp26_25 = S4*S5;
+    ROcp26_35 = -C4*S5;
+    ROcp26_85 = -S4*C5;
+    ROcp26_95 = C4*C5;
+    ROcp26_16 = C5*C6;
+    ROcp26_26 = ROcp26_25*C6+C4*S6;
+    ROcp26_36 = ROcp26_35*C6+S4*S6;
+    ROcp26_46 = -C5*S6;
+    ROcp26_56 = -ROcp26_25*S6+C4*C6;
+    ROcp26_66 = -ROcp26_35*S6+S4*C6;
+    ROcp26_17 = ROcp26_16*C7-S5*S7;
+    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
+    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
+    ROcp26_77 = ROcp26_16*S7+S5*C7;
+    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
+    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
+    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
+    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
+    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
+    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
+    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
+    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
+    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
+    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
+    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
+    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
+    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
+    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
+    OMcp26_25 = qd[5]*C4;
+    OMcp26_35 = qd[5]*S4;
+    OMcp26_16 = qd[4]+qd[6]*S5;
+    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
+    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
+    RLcp26_17 = ROcp26_46*dpt[2][2];
+    RLcp26_27 = ROcp26_56*dpt[2][2];
+    RLcp26_37 = ROcp26_66*dpt[2][2];
+    POcp26_17 = q[1]+RLcp26_17;
+    POcp26_27 = q[2]+RLcp26_27;
+    POcp26_37 = q[3]+RLcp26_37;
+    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
+    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
+    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
+    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
+    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
+    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
+    VIcp26_17 = qd[1]+ORcp26_17;
+    VIcp26_27 = qd[2]+ORcp26_27;
+    VIcp26_37 = qd[3]+ORcp26_37;
+    RLcp26_18 = ROcp26_46*dpt[2][6];
+    RLcp26_28 = ROcp26_56*dpt[2][6];
+    RLcp26_38 = ROcp26_66*dpt[2][6];
+    POcp26_18 = POcp26_17+RLcp26_18;
+    POcp26_28 = POcp26_27+RLcp26_28;
+    POcp26_38 = POcp26_37+RLcp26_38;
+    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
+    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
+    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
+    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
+    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
+    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
+    VIcp26_18 = ORcp26_18+VIcp26_17;
+    VIcp26_28 = ORcp26_28+VIcp26_27;
+    VIcp26_38 = ORcp26_38+VIcp26_37;
+    RLcp26_19 = ROcp26_78*dpt[3][8];
+    RLcp26_29 = ROcp26_88*dpt[3][8];
+    RLcp26_39 = ROcp26_98*dpt[3][8];
+    POcp26_19 = POcp26_18+RLcp26_19;
+    POcp26_29 = POcp26_28+RLcp26_29;
+    POcp26_39 = POcp26_38+RLcp26_39;
+    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
+    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
+    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
+    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
+    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
+    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
+    VIcp26_19 = ORcp26_19+VIcp26_18;
+    VIcp26_29 = ORcp26_29+VIcp26_28;
+    VIcp26_39 = ORcp26_39+VIcp26_38;
+    PxF2[1] = POcp26_19;
+    PxF2[2] = POcp26_29;
+    PxF2[3] = POcp26_39;
+    RxF2[1][1] = ROcp26_19;
+    RxF2[1][2] = ROcp26_29;
+    RxF2[1][3] = ROcp26_39;
+    RxF2[2][1] = ROcp26_49;
+    RxF2[2][2] = ROcp26_59;
+    RxF2[2][3] = ROcp26_69;
+    RxF2[3][1] = ROcp26_78;
+    RxF2[3][2] = ROcp26_88;
+    RxF2[3][3] = ROcp26_98;
+    VxF2[1] = VIcp26_19;
+    VxF2[2] = VIcp26_29;
+    VxF2[3] = VIcp26_39;
+    OMxF2[1] = OMcp26_19;
+    OMxF2[2] = OMcp26_29;
+    OMxF2[3] = OMcp26_39;
+    ROcp27_25 = S4*S5;
+    ROcp27_35 = -C4*S5;
+    ROcp27_85 = -S4*C5;
+    ROcp27_95 = C4*C5;
+    ROcp27_16 = C5*C6;
+    ROcp27_26 = ROcp27_25*C6+C4*S6;
+    ROcp27_36 = ROcp27_35*C6+S4*S6;
+    ROcp27_46 = -C5*S6;
+    ROcp27_56 = -ROcp27_25*S6+C4*C6;
+    ROcp27_66 = -ROcp27_35*S6+S4*C6;
+    ROcp27_17 = ROcp27_16*C7-S5*S7;
+    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
+    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
+    ROcp27_77 = ROcp27_16*S7+S5*C7;
+    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
+    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
+    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
+    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
+    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
+    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
+    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
+    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
+    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
+    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
+    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
+    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
+    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
+    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
+    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
+    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
+    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
+    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
+    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
+    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
+    OMcp27_25 = qd[5]*C4;
+    OMcp27_35 = qd[5]*S4;
+    OMcp27_16 = qd[4]+qd[6]*S5;
+    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
+    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
+    RLcp27_17 = ROcp27_46*dpt[2][2];
+    RLcp27_27 = ROcp27_56*dpt[2][2];
+    RLcp27_37 = ROcp27_66*dpt[2][2];
+    POcp27_17 = q[1]+RLcp27_17;
+    POcp27_27 = q[2]+RLcp27_27;
+    POcp27_37 = q[3]+RLcp27_37;
+    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
+    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
+    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
+    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
+    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
+    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
+    VIcp27_17 = qd[1]+ORcp27_17;
+    VIcp27_27 = qd[2]+ORcp27_27;
+    VIcp27_37 = qd[3]+ORcp27_37;
+    RLcp27_18 = ROcp27_46*dpt[2][6];
+    RLcp27_28 = ROcp27_56*dpt[2][6];
+    RLcp27_38 = ROcp27_66*dpt[2][6];
+    POcp27_18 = POcp27_17+RLcp27_18;
+    POcp27_28 = POcp27_27+RLcp27_28;
+    POcp27_38 = POcp27_37+RLcp27_38;
+    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
+    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
+    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
+    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
+    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
+    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
+    VIcp27_18 = ORcp27_18+VIcp27_17;
+    VIcp27_28 = ORcp27_28+VIcp27_27;
+    VIcp27_38 = ORcp27_38+VIcp27_37;
+    RLcp27_19 = ROcp27_78*dpt[3][8];
+    RLcp27_29 = ROcp27_88*dpt[3][8];
+    RLcp27_39 = ROcp27_98*dpt[3][8];
+    POcp27_19 = POcp27_18+RLcp27_19;
+    POcp27_29 = POcp27_28+RLcp27_29;
+    POcp27_39 = POcp27_38+RLcp27_39;
+    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
+    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
+    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
+    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
+    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
+    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
+    VIcp27_19 = ORcp27_19+VIcp27_18;
+    VIcp27_29 = ORcp27_29+VIcp27_28;
+    VIcp27_39 = ORcp27_39+VIcp27_38;
+    RLcp27_110 = ROcp27_78*dpt[3][11];
+    RLcp27_210 = ROcp27_88*dpt[3][11];
+    RLcp27_310 = ROcp27_98*dpt[3][11];
+    POcp27_110 = POcp27_19+RLcp27_110;
+    POcp27_210 = POcp27_29+RLcp27_210;
+    POcp27_310 = POcp27_39+RLcp27_310;
+    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
+    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
+    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
+    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
+    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
+    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
+    VIcp27_110 = ORcp27_110+VIcp27_19;
+    VIcp27_210 = ORcp27_210+VIcp27_29;
+    VIcp27_310 = ORcp27_310+VIcp27_39;
+    PxF3[1] = POcp27_110;
+    PxF3[2] = POcp27_210;
+    PxF3[3] = POcp27_310;
+    RxF3[1][1] = ROcp27_110;
+    RxF3[1][2] = ROcp27_210;
+    RxF3[1][3] = ROcp27_310;
+    RxF3[2][1] = ROcp27_49;
+    RxF3[2][2] = ROcp27_59;
+    RxF3[2][3] = ROcp27_69;
+    RxF3[3][1] = ROcp27_710;
+    RxF3[3][2] = ROcp27_810;
+    RxF3[3][3] = ROcp27_910;
+    VxF3[1] = VIcp27_110;
+    VxF3[2] = VIcp27_210;
+    VxF3[3] = VIcp27_310;
+    OMxF3[1] = OMcp27_110;
+    OMxF3[2] = OMcp27_210;
+    OMxF3[3] = OMcp27_310;
+    ROcp28_25 = S4*S5;
+    ROcp28_35 = -C4*S5;
+    ROcp28_85 = -S4*C5;
+    ROcp28_95 = C4*C5;
+    ROcp28_16 = C5*C6;
+    ROcp28_26 = ROcp28_25*C6+C4*S6;
+    ROcp28_36 = ROcp28_35*C6+S4*S6;
+    ROcp28_46 = -C5*S6;
+    ROcp28_56 = -ROcp28_25*S6+C4*C6;
+    ROcp28_66 = -ROcp28_35*S6+S4*C6;
+    ROcp28_17 = ROcp28_16*C7-S5*S7;
+    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
+    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
+    ROcp28_77 = ROcp28_16*S7+S5*C7;
+    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
+    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
+    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
+    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
+    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
+    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
+    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
+    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
+    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
+    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
+    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
+    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
+    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
+    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
+    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
+    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
+    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
+    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
+    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
+    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
+    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
+    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
+    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
+    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
+    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
+    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
+    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
+    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
+    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
+    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
+    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
+    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
+    OMcp28_25 = qd[5]*C4;
+    OMcp28_35 = qd[5]*S4;
+    OMcp28_16 = qd[4]+qd[6]*S5;
+    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
+    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
+    RLcp28_17 = ROcp28_46*dpt[2][2];
+    RLcp28_27 = ROcp28_56*dpt[2][2];
+    RLcp28_37 = ROcp28_66*dpt[2][2];
+    POcp28_17 = q[1]+RLcp28_17;
+    POcp28_27 = q[2]+RLcp28_27;
+    POcp28_37 = q[3]+RLcp28_37;
+    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
+    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
+    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
+    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
+    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
+    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
+    VIcp28_17 = qd[1]+ORcp28_17;
+    VIcp28_27 = qd[2]+ORcp28_27;
+    VIcp28_37 = qd[3]+ORcp28_37;
+    RLcp28_18 = ROcp28_46*dpt[2][6];
+    RLcp28_28 = ROcp28_56*dpt[2][6];
+    RLcp28_38 = ROcp28_66*dpt[2][6];
+    POcp28_18 = POcp28_17+RLcp28_18;
+    POcp28_28 = POcp28_27+RLcp28_28;
+    POcp28_38 = POcp28_37+RLcp28_38;
+    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
+    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
+    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
+    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
+    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
+    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
+    VIcp28_18 = ORcp28_18+VIcp28_17;
+    VIcp28_28 = ORcp28_28+VIcp28_27;
+    VIcp28_38 = ORcp28_38+VIcp28_37;
+    RLcp28_19 = ROcp28_78*dpt[3][8];
+    RLcp28_29 = ROcp28_88*dpt[3][8];
+    RLcp28_39 = ROcp28_98*dpt[3][8];
+    POcp28_19 = POcp28_18+RLcp28_19;
+    POcp28_29 = POcp28_28+RLcp28_29;
+    POcp28_39 = POcp28_38+RLcp28_39;
+    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
+    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
+    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
+    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
+    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
+    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
+    VIcp28_19 = ORcp28_19+VIcp28_18;
+    VIcp28_29 = ORcp28_29+VIcp28_28;
+    VIcp28_39 = ORcp28_39+VIcp28_38;
+    RLcp28_110 = ROcp28_78*dpt[3][11];
+    RLcp28_210 = ROcp28_88*dpt[3][11];
+    RLcp28_310 = ROcp28_98*dpt[3][11];
+    POcp28_110 = POcp28_19+RLcp28_110;
+    POcp28_210 = POcp28_29+RLcp28_210;
+    POcp28_310 = POcp28_39+RLcp28_310;
+    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
+    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
+    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
+    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
+    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
+    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
+    VIcp28_110 = ORcp28_110+VIcp28_19;
+    VIcp28_210 = ORcp28_210+VIcp28_29;
+    VIcp28_310 = ORcp28_310+VIcp28_39;
+    RLcp28_111 = ROcp28_710*dpt[3][14];
+    RLcp28_211 = ROcp28_810*dpt[3][14];
+    RLcp28_311 = ROcp28_910*dpt[3][14];
+    POcp28_111 = POcp28_110+RLcp28_111;
+    POcp28_211 = POcp28_210+RLcp28_211;
+    POcp28_311 = POcp28_310+RLcp28_311;
+    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
+    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
+    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
+    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
+    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
+    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
+    VIcp28_111 = ORcp28_111+VIcp28_110;
+    VIcp28_211 = ORcp28_211+VIcp28_210;
+    VIcp28_311 = ORcp28_311+VIcp28_310;
+    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
+    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
+    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
+    RLcp28_113 = ROcp28_712*dpt[3][19];
+    RLcp28_213 = ROcp28_812*dpt[3][19];
+    RLcp28_313 = ROcp28_912*dpt[3][19];
+    POcp28_113 = POcp28_111+RLcp28_113;
+    POcp28_213 = POcp28_211+RLcp28_213;
+    POcp28_313 = POcp28_311+RLcp28_313;
+    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
+    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
+    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
+    VIcp28_113 = ORcp28_113+VIcp28_111;
+    VIcp28_213 = ORcp28_213+VIcp28_211;
+    VIcp28_313 = ORcp28_313+VIcp28_311;
+    PxF4[1] = POcp28_113;
+    PxF4[2] = POcp28_213;
+    PxF4[3] = POcp28_313;
+    RxF4[1][1] = ROcp28_112;
+    RxF4[1][2] = ROcp28_212;
+    RxF4[1][3] = ROcp28_312;
+    RxF4[2][1] = ROcp28_411;
+    RxF4[2][2] = ROcp28_511;
+    RxF4[2][3] = ROcp28_611;
+    RxF4[3][1] = ROcp28_712;
+    RxF4[3][2] = ROcp28_812;
+    RxF4[3][3] = ROcp28_912;
+    VxF4[1] = VIcp28_113;
+    VxF4[2] = VIcp28_213;
+    VxF4[3] = VIcp28_313;
+    OMxF4[1] = OMcp28_112;
+    OMxF4[2] = OMcp28_212;
+    OMxF4[3] = OMcp28_312;
+    ROcp29_25 = S4*S5;
+    ROcp29_35 = -C4*S5;
+    ROcp29_85 = -S4*C5;
+    ROcp29_95 = C4*C5;
+    ROcp29_16 = C5*C6;
+    ROcp29_26 = ROcp29_25*C6+C4*S6;
+    ROcp29_36 = ROcp29_35*C6+S4*S6;
+    ROcp29_46 = -C5*S6;
+    ROcp29_56 = -ROcp29_25*S6+C4*C6;
+    ROcp29_66 = -ROcp29_35*S6+S4*C6;
+    ROcp29_113 = ROcp29_16*C13-S13*S5;
+    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
+    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
+    ROcp29_713 = ROcp29_16*S13+C13*S5;
+    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
+    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
+    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
+    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
+    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
+    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
+    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
+    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
+    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
+    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
+    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
+    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
+    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
+    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
+    OMcp29_25 = qd[5]*C4;
+    OMcp29_35 = qd[5]*S4;
+    OMcp29_16 = qd[4]+qd[6]*S5;
+    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
+    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
+    RLcp29_17 = ROcp29_46*dpt[2][3];
+    RLcp29_27 = ROcp29_56*dpt[2][3];
+    RLcp29_37 = ROcp29_66*dpt[2][3];
+    POcp29_17 = q[1]+RLcp29_17;
+    POcp29_27 = q[2]+RLcp29_27;
+    POcp29_37 = q[3]+RLcp29_37;
+    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
+    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
+    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
+    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
+    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
+    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
+    VIcp29_17 = qd[1]+ORcp29_17;
+    VIcp29_27 = qd[2]+ORcp29_27;
+    VIcp29_37 = qd[3]+ORcp29_37;
+    RLcp29_18 = ROcp29_46*dpt[2][20];
+    RLcp29_28 = ROcp29_56*dpt[2][20];
+    RLcp29_38 = ROcp29_66*dpt[2][20];
+    POcp29_18 = POcp29_17+RLcp29_18;
+    POcp29_28 = POcp29_27+RLcp29_28;
+    POcp29_38 = POcp29_37+RLcp29_38;
+    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
+    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
+    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
+    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
+    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
+    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
+    VIcp29_18 = ORcp29_18+VIcp29_17;
+    VIcp29_28 = ORcp29_28+VIcp29_27;
+    VIcp29_38 = ORcp29_38+VIcp29_37;
+    RLcp29_19 = ROcp29_714*dpt[3][22];
+    RLcp29_29 = ROcp29_814*dpt[3][22];
+    RLcp29_39 = ROcp29_914*dpt[3][22];
+    POcp29_19 = POcp29_18+RLcp29_19;
+    POcp29_29 = POcp29_28+RLcp29_29;
+    POcp29_39 = POcp29_38+RLcp29_39;
+    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
+    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
+    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
+    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
+    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
+    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
+    VIcp29_19 = ORcp29_19+VIcp29_18;
+    VIcp29_29 = ORcp29_29+VIcp29_28;
+    VIcp29_39 = ORcp29_39+VIcp29_38;
+    PxF5[1] = POcp29_19;
+    PxF5[2] = POcp29_29;
+    PxF5[3] = POcp29_39;
+    RxF5[1][1] = ROcp29_115;
+    RxF5[1][2] = ROcp29_215;
+    RxF5[1][3] = ROcp29_315;
+    RxF5[2][1] = ROcp29_415;
+    RxF5[2][2] = ROcp29_515;
+    RxF5[2][3] = ROcp29_615;
+    RxF5[3][1] = ROcp29_714;
+    RxF5[3][2] = ROcp29_814;
+    RxF5[3][3] = ROcp29_914;
+    VxF5[1] = VIcp29_19;
+    VxF5[2] = VIcp29_29;
+    VxF5[3] = VIcp29_39;
+    OMxF5[1] = OMcp29_19;
+    OMxF5[2] = OMcp29_29;
+    OMxF5[3] = OMcp29_39;
+    ROcp30_25 = S4*S5;
+    ROcp30_35 = -C4*S5;
+    ROcp30_85 = -S4*C5;
+    ROcp30_95 = C4*C5;
+    ROcp30_16 = C5*C6;
+    ROcp30_26 = ROcp30_25*C6+C4*S6;
+    ROcp30_36 = ROcp30_35*C6+S4*S6;
+    ROcp30_46 = -C5*S6;
+    ROcp30_56 = -ROcp30_25*S6+C4*C6;
+    ROcp30_66 = -ROcp30_35*S6+S4*C6;
+    ROcp30_113 = ROcp30_16*C13-S13*S5;
+    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
+    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
+    ROcp30_713 = ROcp30_16*S13+C13*S5;
+    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
+    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
+    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
+    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
+    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
+    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
+    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
+    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
+    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
+    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
+    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
+    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
+    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
+    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
+    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
+    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
+    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
+    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
+    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
+    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
+    OMcp30_25 = qd[5]*C4;
+    OMcp30_35 = qd[5]*S4;
+    OMcp30_16 = qd[4]+qd[6]*S5;
+    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
+    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
+    RLcp30_17 = ROcp30_46*dpt[2][3];
+    RLcp30_27 = ROcp30_56*dpt[2][3];
+    RLcp30_37 = ROcp30_66*dpt[2][3];
+    POcp30_17 = q[1]+RLcp30_17;
+    POcp30_27 = q[2]+RLcp30_27;
+    POcp30_37 = q[3]+RLcp30_37;
+    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
+    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
+    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
+    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
+    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
+    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
+    VIcp30_17 = qd[1]+ORcp30_17;
+    VIcp30_27 = qd[2]+ORcp30_27;
+    VIcp30_37 = qd[3]+ORcp30_37;
+    RLcp30_18 = ROcp30_46*dpt[2][20];
+    RLcp30_28 = ROcp30_56*dpt[2][20];
+    RLcp30_38 = ROcp30_66*dpt[2][20];
+    POcp30_18 = POcp30_17+RLcp30_18;
+    POcp30_28 = POcp30_27+RLcp30_28;
+    POcp30_38 = POcp30_37+RLcp30_38;
+    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
+    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
+    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
+    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
+    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
+    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
+    VIcp30_18 = ORcp30_18+VIcp30_17;
+    VIcp30_28 = ORcp30_28+VIcp30_27;
+    VIcp30_38 = ORcp30_38+VIcp30_37;
+    RLcp30_19 = ROcp30_714*dpt[3][22];
+    RLcp30_29 = ROcp30_814*dpt[3][22];
+    RLcp30_39 = ROcp30_914*dpt[3][22];
+    POcp30_19 = POcp30_18+RLcp30_19;
+    POcp30_29 = POcp30_28+RLcp30_29;
+    POcp30_39 = POcp30_38+RLcp30_39;
+    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
+    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
+    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
+    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
+    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
+    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
+    VIcp30_19 = ORcp30_19+VIcp30_18;
+    VIcp30_29 = ORcp30_29+VIcp30_28;
+    VIcp30_39 = ORcp30_39+VIcp30_38;
+    RLcp30_110 = ROcp30_714*dpt[3][25];
+    RLcp30_210 = ROcp30_814*dpt[3][25];
+    RLcp30_310 = ROcp30_914*dpt[3][25];
+    POcp30_110 = POcp30_19+RLcp30_110;
+    POcp30_210 = POcp30_29+RLcp30_210;
+    POcp30_310 = POcp30_39+RLcp30_310;
+    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
+    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
+    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
+    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
+    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
+    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
+    VIcp30_110 = ORcp30_110+VIcp30_19;
+    VIcp30_210 = ORcp30_210+VIcp30_29;
+    VIcp30_310 = ORcp30_310+VIcp30_39;
+    PxF6[1] = POcp30_110;
+    PxF6[2] = POcp30_210;
+    PxF6[3] = POcp30_310;
+    RxF6[1][1] = ROcp30_116;
+    RxF6[1][2] = ROcp30_216;
+    RxF6[1][3] = ROcp30_316;
+    RxF6[2][1] = ROcp30_415;
+    RxF6[2][2] = ROcp30_515;
+    RxF6[2][3] = ROcp30_615;
+    RxF6[3][1] = ROcp30_716;
+    RxF6[3][2] = ROcp30_816;
+    RxF6[3][3] = ROcp30_916;
+    VxF6[1] = VIcp30_110;
+    VxF6[2] = VIcp30_210;
+    VxF6[3] = VIcp30_310;
+    OMxF6[1] = OMcp30_110;
+    OMxF6[2] = OMcp30_210;
+    OMxF6[3] = OMcp30_310;
+    ROcp31_25 = S4*S5;
+    ROcp31_35 = -C4*S5;
+    ROcp31_85 = -S4*C5;
+    ROcp31_95 = C4*C5;
+    ROcp31_16 = C5*C6;
+    ROcp31_26 = ROcp31_25*C6+C4*S6;
+    ROcp31_36 = ROcp31_35*C6+S4*S6;
+    ROcp31_46 = -C5*S6;
+    ROcp31_56 = -ROcp31_25*S6+C4*C6;
+    ROcp31_66 = -ROcp31_35*S6+S4*C6;
+    ROcp31_113 = ROcp31_16*C13-S13*S5;
+    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
+    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
+    ROcp31_713 = ROcp31_16*S13+C13*S5;
+    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
+    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
+    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
+    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
+    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
+    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
+    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
+    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
+    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
+    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
+    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
+    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
+    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
+    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
+    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
+    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
+    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
+    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
+    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
+    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
+    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
+    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
+    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
+    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
+    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
+    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
+    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
+    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
+    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
+    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
+    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
+    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
+    OMcp31_25 = qd[5]*C4;
+    OMcp31_35 = qd[5]*S4;
+    OMcp31_16 = qd[4]+qd[6]*S5;
+    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
+    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
+    RLcp31_17 = ROcp31_46*dpt[2][3];
+    RLcp31_27 = ROcp31_56*dpt[2][3];
+    RLcp31_37 = ROcp31_66*dpt[2][3];
+    POcp31_17 = q[1]+RLcp31_17;
+    POcp31_27 = q[2]+RLcp31_27;
+    POcp31_37 = q[3]+RLcp31_37;
+    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
+    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
+    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
+    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
+    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
+    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
+    VIcp31_17 = qd[1]+ORcp31_17;
+    VIcp31_27 = qd[2]+ORcp31_27;
+    VIcp31_37 = qd[3]+ORcp31_37;
+    RLcp31_18 = ROcp31_46*dpt[2][20];
+    RLcp31_28 = ROcp31_56*dpt[2][20];
+    RLcp31_38 = ROcp31_66*dpt[2][20];
+    POcp31_18 = POcp31_17+RLcp31_18;
+    POcp31_28 = POcp31_27+RLcp31_28;
+    POcp31_38 = POcp31_37+RLcp31_38;
+    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
+    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
+    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
+    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
+    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
+    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
+    VIcp31_18 = ORcp31_18+VIcp31_17;
+    VIcp31_28 = ORcp31_28+VIcp31_27;
+    VIcp31_38 = ORcp31_38+VIcp31_37;
+    RLcp31_19 = ROcp31_714*dpt[3][22];
+    RLcp31_29 = ROcp31_814*dpt[3][22];
+    RLcp31_39 = ROcp31_914*dpt[3][22];
+    POcp31_19 = POcp31_18+RLcp31_19;
+    POcp31_29 = POcp31_28+RLcp31_29;
+    POcp31_39 = POcp31_38+RLcp31_39;
+    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
+    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
+    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
+    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
+    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
+    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
+    VIcp31_19 = ORcp31_19+VIcp31_18;
+    VIcp31_29 = ORcp31_29+VIcp31_28;
+    VIcp31_39 = ORcp31_39+VIcp31_38;
+    RLcp31_110 = ROcp31_714*dpt[3][25];
+    RLcp31_210 = ROcp31_814*dpt[3][25];
+    RLcp31_310 = ROcp31_914*dpt[3][25];
+    POcp31_110 = POcp31_19+RLcp31_110;
+    POcp31_210 = POcp31_29+RLcp31_210;
+    POcp31_310 = POcp31_39+RLcp31_310;
+    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
+    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
+    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
+    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
+    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
+    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
+    VIcp31_110 = ORcp31_110+VIcp31_19;
+    VIcp31_210 = ORcp31_210+VIcp31_29;
+    VIcp31_310 = ORcp31_310+VIcp31_39;
+    RLcp31_111 = ROcp31_716*dpt[3][28];
+    RLcp31_211 = ROcp31_816*dpt[3][28];
+    RLcp31_311 = ROcp31_916*dpt[3][28];
+    POcp31_111 = POcp31_110+RLcp31_111;
+    POcp31_211 = POcp31_210+RLcp31_211;
+    POcp31_311 = POcp31_310+RLcp31_311;
+    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
+    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
+    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
+    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
+    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
+    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
+    VIcp31_111 = ORcp31_111+VIcp31_110;
+    VIcp31_211 = ORcp31_211+VIcp31_210;
+    VIcp31_311 = ORcp31_311+VIcp31_310;
+    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
+    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
+    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
+    RLcp31_113 = ROcp31_718*dpt[3][33];
+    RLcp31_213 = ROcp31_818*dpt[3][33];
+    RLcp31_313 = ROcp31_918*dpt[3][33];
+    POcp31_113 = POcp31_111+RLcp31_113;
+    POcp31_213 = POcp31_211+RLcp31_213;
+    POcp31_313 = POcp31_311+RLcp31_313;
+    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
+    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
+    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
+    VIcp31_113 = ORcp31_113+VIcp31_111;
+    VIcp31_213 = ORcp31_213+VIcp31_211;
+    VIcp31_313 = ORcp31_313+VIcp31_311;
+    PxF7[1] = POcp31_113;
+    PxF7[2] = POcp31_213;
+    PxF7[3] = POcp31_313;
+    RxF7[1][1] = ROcp31_118;
+    RxF7[1][2] = ROcp31_218;
+    RxF7[1][3] = ROcp31_318;
+    RxF7[2][1] = ROcp31_417;
+    RxF7[2][2] = ROcp31_517;
+    RxF7[2][3] = ROcp31_617;
+    RxF7[3][1] = ROcp31_718;
+    RxF7[3][2] = ROcp31_818;
+    RxF7[3][3] = ROcp31_918;
+    VxF7[1] = VIcp31_113;
+    VxF7[2] = VIcp31_213;
+    VxF7[3] = VIcp31_313;
+    OMxF7[1] = OMcp31_112;
+    OMxF7[2] = OMcp31_212;
+    OMxF7[3] = OMcp31_312;
+    ROcp32_25 = S4*S5;
+    ROcp32_35 = -C4*S5;
+    ROcp32_85 = -S4*C5;
+    ROcp32_95 = C4*C5;
+    ROcp32_16 = C5*C6;
+    ROcp32_26 = ROcp32_25*C6+C4*S6;
+    ROcp32_36 = ROcp32_35*C6+S4*S6;
+    ROcp32_46 = -C5*S6;
+    ROcp32_56 = -ROcp32_25*S6+C4*C6;
+    ROcp32_66 = -ROcp32_35*S6+S4*C6;
+    ROcp32_419 = ROcp32_46*C19+S19*S5;
+    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
+    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
+    ROcp32_719 = -ROcp32_46*S19+C19*S5;
+    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
+    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
+    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
+    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
+    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
+    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
+    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
+    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
+    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
+    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
+    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
+    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
+    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
+    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
+    OMcp32_25 = qd[5]*C4;
+    OMcp32_35 = qd[5]*S4;
+    OMcp32_16 = qd[4]+qd[6]*S5;
+    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
+    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
+    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
+    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
+    POcp32_17 = q[1]+RLcp32_17;
+    POcp32_27 = q[2]+RLcp32_27;
+    POcp32_37 = q[3]+RLcp32_37;
+    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
+    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
+    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
+    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
+    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
+    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
+    VIcp32_17 = qd[1]+ORcp32_17;
+    VIcp32_27 = qd[2]+ORcp32_27;
+    VIcp32_37 = qd[3]+ORcp32_37;
+    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
+    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
+    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
+    RLcp32_19 = ROcp32_720*dpt[3][36];
+    RLcp32_29 = ROcp32_820*dpt[3][36];
+    RLcp32_39 = ROcp32_920*dpt[3][36];
+    POcp32_19 = POcp32_17+RLcp32_19;
+    POcp32_29 = POcp32_27+RLcp32_29;
+    POcp32_39 = POcp32_37+RLcp32_39;
+    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
+    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
+    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
+    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
+    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
+    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
+    VIcp32_19 = ORcp32_19+VIcp32_17;
+    VIcp32_29 = ORcp32_29+VIcp32_27;
+    VIcp32_39 = ORcp32_39+VIcp32_37;
+    PxF8[1] = POcp32_19;
+    PxF8[2] = POcp32_29;
+    PxF8[3] = POcp32_39;
+    RxF8[1][1] = ROcp32_121;
+    RxF8[1][2] = ROcp32_221;
+    RxF8[1][3] = ROcp32_321;
+    RxF8[2][1] = ROcp32_421;
+    RxF8[2][2] = ROcp32_521;
+    RxF8[2][3] = ROcp32_621;
+    RxF8[3][1] = ROcp32_720;
+    RxF8[3][2] = ROcp32_820;
+    RxF8[3][3] = ROcp32_920;
+    VxF8[1] = VIcp32_19;
+    VxF8[2] = VIcp32_29;
+    VxF8[3] = VIcp32_39;
+    OMxF8[1] = OMcp32_19;
+    OMxF8[2] = OMcp32_29;
+    OMxF8[3] = OMcp32_39;
+    ROcp33_25 = S4*S5;
+    ROcp33_35 = -C4*S5;
+    ROcp33_85 = -S4*C5;
+    ROcp33_95 = C4*C5;
+    ROcp33_16 = C5*C6;
+    ROcp33_26 = ROcp33_25*C6+C4*S6;
+    ROcp33_36 = ROcp33_35*C6+S4*S6;
+    ROcp33_46 = -C5*S6;
+    ROcp33_56 = -ROcp33_25*S6+C4*C6;
+    ROcp33_66 = -ROcp33_35*S6+S4*C6;
+    ROcp33_419 = ROcp33_46*C19+S19*S5;
+    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
+    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
+    ROcp33_719 = -ROcp33_46*S19+C19*S5;
+    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
+    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
+    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
+    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
+    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
+    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
+    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
+    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
+    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
+    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
+    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
+    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
+    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
+    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
+    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
+    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
+    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
+    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
+    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
+    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
+    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
+    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
+    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
+    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
+    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
+    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
+    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
+    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
+    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
+    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
+    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
+    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
+    OMcp33_25 = qd[5]*C4;
+    OMcp33_35 = qd[5]*S4;
+    OMcp33_16 = qd[4]+qd[6]*S5;
+    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
+    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
+    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
+    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
+    POcp33_17 = q[1]+RLcp33_17;
+    POcp33_27 = q[2]+RLcp33_27;
+    POcp33_37 = q[3]+RLcp33_37;
+    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
+    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
+    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
+    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
+    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
+    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
+    VIcp33_17 = qd[1]+ORcp33_17;
+    VIcp33_27 = qd[2]+ORcp33_27;
+    VIcp33_37 = qd[3]+ORcp33_37;
+    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
+    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
+    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
+    RLcp33_19 = ROcp33_720*dpt[3][36];
+    RLcp33_29 = ROcp33_820*dpt[3][36];
+    RLcp33_39 = ROcp33_920*dpt[3][36];
+    POcp33_19 = POcp33_17+RLcp33_19;
+    POcp33_29 = POcp33_27+RLcp33_29;
+    POcp33_39 = POcp33_37+RLcp33_39;
+    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
+    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
+    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
+    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
+    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
+    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
+    VIcp33_19 = ORcp33_19+VIcp33_17;
+    VIcp33_29 = ORcp33_29+VIcp33_27;
+    VIcp33_39 = ORcp33_39+VIcp33_37;
+    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
+    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
+    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
+    POcp33_110 = POcp33_19+RLcp33_110;
+    POcp33_210 = POcp33_29+RLcp33_210;
+    POcp33_310 = POcp33_39+RLcp33_310;
+    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
+    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
+    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
+    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
+    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
+    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
+    VIcp33_110 = ORcp33_110+VIcp33_19;
+    VIcp33_210 = ORcp33_210+VIcp33_29;
+    VIcp33_310 = ORcp33_310+VIcp33_39;
+    RLcp33_111 = ROcp33_421*dpt[2][43];
+    RLcp33_211 = ROcp33_521*dpt[2][43];
+    RLcp33_311 = ROcp33_621*dpt[2][43];
+    POcp33_111 = POcp33_110+RLcp33_111;
+    POcp33_211 = POcp33_210+RLcp33_211;
+    POcp33_311 = POcp33_310+RLcp33_311;
+    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
+    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
+    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
+    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
+    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
+    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
+    VIcp33_111 = ORcp33_111+VIcp33_110;
+    VIcp33_211 = ORcp33_211+VIcp33_210;
+    VIcp33_311 = ORcp33_311+VIcp33_310;
+    RLcp33_112 = ROcp33_723*dpt[3][45];
+    RLcp33_212 = ROcp33_823*dpt[3][45];
+    RLcp33_312 = ROcp33_923*dpt[3][45];
+    POcp33_112 = POcp33_111+RLcp33_112;
+    POcp33_212 = POcp33_211+RLcp33_212;
+    POcp33_312 = POcp33_311+RLcp33_312;
+    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
+    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
+    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
+    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
+    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
+    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
+    VIcp33_112 = ORcp33_112+VIcp33_111;
+    VIcp33_212 = ORcp33_212+VIcp33_211;
+    VIcp33_312 = ORcp33_312+VIcp33_311;
+    PxF9[1] = POcp33_112;
+    PxF9[2] = POcp33_212;
+    PxF9[3] = POcp33_312;
+    RxF9[1][1] = ROcp33_124;
+    RxF9[1][2] = ROcp33_224;
+    RxF9[1][3] = ROcp33_324;
+    RxF9[2][1] = ROcp33_424;
+    RxF9[2][2] = ROcp33_524;
+    RxF9[2][3] = ROcp33_624;
+    RxF9[3][1] = ROcp33_723;
+    RxF9[3][2] = ROcp33_823;
+    RxF9[3][3] = ROcp33_923;
+    VxF9[1] = VIcp33_112;
+    VxF9[2] = VIcp33_212;
+    VxF9[3] = VIcp33_312;
+    OMxF9[1] = OMcp33_112;
+    OMxF9[2] = OMcp33_212;
+    OMxF9[3] = OMcp33_312;
+    ROcp34_25 = S4*S5;
+    ROcp34_35 = -C4*S5;
+    ROcp34_85 = -S4*C5;
+    ROcp34_95 = C4*C5;
+    ROcp34_16 = C5*C6;
+    ROcp34_26 = ROcp34_25*C6+C4*S6;
+    ROcp34_36 = ROcp34_35*C6+S4*S6;
+    ROcp34_46 = -C5*S6;
+    ROcp34_56 = -ROcp34_25*S6+C4*C6;
+    ROcp34_66 = -ROcp34_35*S6+S4*C6;
+    ROcp34_419 = ROcp34_46*C19+S19*S5;
+    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
+    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
+    ROcp34_719 = -ROcp34_46*S19+C19*S5;
+    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
+    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
+    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
+    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
+    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
+    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
+    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
+    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
+    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
+    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
+    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
+    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
+    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
+    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
+    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
+    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
+    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
+    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
+    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
+    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
+    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
+    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
+    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
+    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
+    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
+    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
+    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
+    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
+    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
+    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
+    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
+    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
+    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
+    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
+    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
+    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
+    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
+    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
+    OMcp34_25 = qd[5]*C4;
+    OMcp34_35 = qd[5]*S4;
+    OMcp34_16 = qd[4]+qd[6]*S5;
+    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
+    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
+    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
+    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
+    POcp34_17 = q[1]+RLcp34_17;
+    POcp34_27 = q[2]+RLcp34_27;
+    POcp34_37 = q[3]+RLcp34_37;
+    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
+    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
+    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
+    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
+    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
+    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
+    VIcp34_17 = qd[1]+ORcp34_17;
+    VIcp34_27 = qd[2]+ORcp34_27;
+    VIcp34_37 = qd[3]+ORcp34_37;
+    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
+    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
+    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
+    RLcp34_19 = ROcp34_720*dpt[3][36];
+    RLcp34_29 = ROcp34_820*dpt[3][36];
+    RLcp34_39 = ROcp34_920*dpt[3][36];
+    POcp34_19 = POcp34_17+RLcp34_19;
+    POcp34_29 = POcp34_27+RLcp34_29;
+    POcp34_39 = POcp34_37+RLcp34_39;
+    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
+    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
+    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
+    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
+    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
+    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
+    VIcp34_19 = ORcp34_19+VIcp34_17;
+    VIcp34_29 = ORcp34_29+VIcp34_27;
+    VIcp34_39 = ORcp34_39+VIcp34_37;
+    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
+    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
+    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
+    POcp34_110 = POcp34_19+RLcp34_110;
+    POcp34_210 = POcp34_29+RLcp34_210;
+    POcp34_310 = POcp34_39+RLcp34_310;
+    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
+    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
+    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
+    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
+    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
+    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
+    VIcp34_110 = ORcp34_110+VIcp34_19;
+    VIcp34_210 = ORcp34_210+VIcp34_29;
+    VIcp34_310 = ORcp34_310+VIcp34_39;
+    RLcp34_111 = ROcp34_421*dpt[2][43];
+    RLcp34_211 = ROcp34_521*dpt[2][43];
+    RLcp34_311 = ROcp34_621*dpt[2][43];
+    POcp34_111 = POcp34_110+RLcp34_111;
+    POcp34_211 = POcp34_210+RLcp34_211;
+    POcp34_311 = POcp34_310+RLcp34_311;
+    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
+    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
+    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
+    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
+    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
+    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
+    VIcp34_111 = ORcp34_111+VIcp34_110;
+    VIcp34_211 = ORcp34_211+VIcp34_210;
+    VIcp34_311 = ORcp34_311+VIcp34_310;
+    RLcp34_112 = ROcp34_723*dpt[3][45];
+    RLcp34_212 = ROcp34_823*dpt[3][45];
+    RLcp34_312 = ROcp34_923*dpt[3][45];
+    POcp34_112 = POcp34_111+RLcp34_112;
+    POcp34_212 = POcp34_211+RLcp34_212;
+    POcp34_312 = POcp34_311+RLcp34_312;
+    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
+    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
+    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
+    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
+    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
+    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
+    VIcp34_112 = ORcp34_112+VIcp34_111;
+    VIcp34_212 = ORcp34_212+VIcp34_211;
+    VIcp34_312 = ORcp34_312+VIcp34_311;
+    RLcp34_113 = ROcp34_723*dpt[3][48];
+    RLcp34_213 = ROcp34_823*dpt[3][48];
+    RLcp34_313 = ROcp34_923*dpt[3][48];
+    POcp34_113 = POcp34_112+RLcp34_113;
+    POcp34_213 = POcp34_212+RLcp34_213;
+    POcp34_313 = POcp34_312+RLcp34_313;
+    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
+    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
+    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
+    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
+    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
+    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
+    VIcp34_113 = ORcp34_113+VIcp34_112;
+    VIcp34_213 = ORcp34_213+VIcp34_212;
+    VIcp34_313 = ORcp34_313+VIcp34_312;
+    PxF10[1] = POcp34_113;
+    PxF10[2] = POcp34_213;
+    PxF10[3] = POcp34_313;
+    RxF10[1][1] = ROcp34_125;
+    RxF10[1][2] = ROcp34_225;
+    RxF10[1][3] = ROcp34_325;
+    RxF10[2][1] = ROcp34_424;
+    RxF10[2][2] = ROcp34_524;
+    RxF10[2][3] = ROcp34_624;
+    RxF10[3][1] = ROcp34_725;
+    RxF10[3][2] = ROcp34_825;
+    RxF10[3][3] = ROcp34_925;
+    VxF10[1] = VIcp34_113;
+    VxF10[2] = VIcp34_213;
+    VxF10[3] = VIcp34_313;
+    OMxF10[1] = OMcp34_113;
+    OMxF10[2] = OMcp34_213;
+    OMxF10[3] = OMcp34_313;
+    ROcp35_25 = S4*S5;
+    ROcp35_35 = -C4*S5;
+    ROcp35_85 = -S4*C5;
+    ROcp35_95 = C4*C5;
+    ROcp35_16 = C5*C6;
+    ROcp35_26 = ROcp35_25*C6+C4*S6;
+    ROcp35_36 = ROcp35_35*C6+S4*S6;
+    ROcp35_46 = -C5*S6;
+    ROcp35_56 = -ROcp35_25*S6+C4*C6;
+    ROcp35_66 = -ROcp35_35*S6+S4*C6;
+    ROcp35_419 = ROcp35_46*C19+S19*S5;
+    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
+    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
+    ROcp35_719 = -ROcp35_46*S19+C19*S5;
+    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
+    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
+    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
+    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
+    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
+    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
+    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
+    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
+    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
+    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
+    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
+    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
+    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
+    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
+    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
+    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
+    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
+    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
+    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
+    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
+    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
+    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
+    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
+    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
+    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
+    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
+    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
+    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
+    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
+    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
+    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
+    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
+    OMcp35_25 = qd[5]*C4;
+    OMcp35_35 = qd[5]*S4;
+    OMcp35_16 = qd[4]+qd[6]*S5;
+    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
+    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
+    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
+    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
+    POcp35_17 = q[1]+RLcp35_17;
+    POcp35_27 = q[2]+RLcp35_27;
+    POcp35_37 = q[3]+RLcp35_37;
+    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
+    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
+    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
+    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
+    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
+    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
+    VIcp35_17 = qd[1]+ORcp35_17;
+    VIcp35_27 = qd[2]+ORcp35_27;
+    VIcp35_37 = qd[3]+ORcp35_37;
+    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
+    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
+    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
+    RLcp35_19 = ROcp35_720*dpt[3][36];
+    RLcp35_29 = ROcp35_820*dpt[3][36];
+    RLcp35_39 = ROcp35_920*dpt[3][36];
+    POcp35_19 = POcp35_17+RLcp35_19;
+    POcp35_29 = POcp35_27+RLcp35_29;
+    POcp35_39 = POcp35_37+RLcp35_39;
+    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
+    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
+    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
+    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
+    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
+    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
+    VIcp35_19 = ORcp35_19+VIcp35_17;
+    VIcp35_29 = ORcp35_29+VIcp35_27;
+    VIcp35_39 = ORcp35_39+VIcp35_37;
+    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
+    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
+    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
+    POcp35_110 = POcp35_19+RLcp35_110;
+    POcp35_210 = POcp35_29+RLcp35_210;
+    POcp35_310 = POcp35_39+RLcp35_310;
+    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
+    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
+    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
+    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
+    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
+    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
+    VIcp35_110 = ORcp35_110+VIcp35_19;
+    VIcp35_210 = ORcp35_210+VIcp35_29;
+    VIcp35_310 = ORcp35_310+VIcp35_39;
+    RLcp35_111 = ROcp35_421*dpt[2][52];
+    RLcp35_211 = ROcp35_521*dpt[2][52];
+    RLcp35_311 = ROcp35_621*dpt[2][52];
+    POcp35_111 = POcp35_110+RLcp35_111;
+    POcp35_211 = POcp35_210+RLcp35_211;
+    POcp35_311 = POcp35_310+RLcp35_311;
+    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
+    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
+    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
+    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
+    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
+    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
+    VIcp35_111 = ORcp35_111+VIcp35_110;
+    VIcp35_211 = ORcp35_211+VIcp35_210;
+    VIcp35_311 = ORcp35_311+VIcp35_310;
+    RLcp35_112 = ROcp35_727*dpt[3][54];
+    RLcp35_212 = ROcp35_827*dpt[3][54];
+    RLcp35_312 = ROcp35_927*dpt[3][54];
+    POcp35_112 = POcp35_111+RLcp35_112;
+    POcp35_212 = POcp35_211+RLcp35_212;
+    POcp35_312 = POcp35_311+RLcp35_312;
+    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
+    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
+    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
+    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
+    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
+    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
+    VIcp35_112 = ORcp35_112+VIcp35_111;
+    VIcp35_212 = ORcp35_212+VIcp35_211;
+    VIcp35_312 = ORcp35_312+VIcp35_311;
+    PxF11[1] = POcp35_112;
+    PxF11[2] = POcp35_212;
+    PxF11[3] = POcp35_312;
+    RxF11[1][1] = ROcp35_128;
+    RxF11[1][2] = ROcp35_228;
+    RxF11[1][3] = ROcp35_328;
+    RxF11[2][1] = ROcp35_428;
+    RxF11[2][2] = ROcp35_528;
+    RxF11[2][3] = ROcp35_628;
+    RxF11[3][1] = ROcp35_727;
+    RxF11[3][2] = ROcp35_827;
+    RxF11[3][3] = ROcp35_927;
+    VxF11[1] = VIcp35_112;
+    VxF11[2] = VIcp35_212;
+    VxF11[3] = VIcp35_312;
+    OMxF11[1] = OMcp35_112;
+    OMxF11[2] = OMcp35_212;
+    OMxF11[3] = OMcp35_312;
+    ROcp36_25 = S4*S5;
+    ROcp36_35 = -C4*S5;
+    ROcp36_85 = -S4*C5;
+    ROcp36_95 = C4*C5;
+    ROcp36_16 = C5*C6;
+    ROcp36_26 = ROcp36_25*C6+C4*S6;
+    ROcp36_36 = ROcp36_35*C6+S4*S6;
+    ROcp36_46 = -C5*S6;
+    ROcp36_56 = -ROcp36_25*S6+C4*C6;
+    ROcp36_66 = -ROcp36_35*S6+S4*C6;
+    ROcp36_419 = ROcp36_46*C19+S19*S5;
+    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
+    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
+    ROcp36_719 = -ROcp36_46*S19+C19*S5;
+    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
+    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
+    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
+    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
+    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
+    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
+    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
+    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
+    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
+    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
+    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
+    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
+    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
+    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
+    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
+    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
+    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
+    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
+    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
+    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
+    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
+    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
+    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
+    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
+    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
+    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
+    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
+    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
+    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
+    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
+    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
+    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
+    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
+    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
+    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
+    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
+    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
+    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
+    OMcp36_25 = qd[5]*C4;
+    OMcp36_35 = qd[5]*S4;
+    OMcp36_16 = qd[4]+qd[6]*S5;
+    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
+    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
+    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
+    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
+    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
+    POcp36_17 = q[1]+RLcp36_17;
+    POcp36_27 = q[2]+RLcp36_27;
+    POcp36_37 = q[3]+RLcp36_37;
+    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
+    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
+    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
+    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
+    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
+    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
+    VIcp36_17 = qd[1]+ORcp36_17;
+    VIcp36_27 = qd[2]+ORcp36_27;
+    VIcp36_37 = qd[3]+ORcp36_37;
+    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
+    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
+    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
+    RLcp36_19 = ROcp36_720*dpt[3][36];
+    RLcp36_29 = ROcp36_820*dpt[3][36];
+    RLcp36_39 = ROcp36_920*dpt[3][36];
+    POcp36_19 = POcp36_17+RLcp36_19;
+    POcp36_29 = POcp36_27+RLcp36_29;
+    POcp36_39 = POcp36_37+RLcp36_39;
+    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
+    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
+    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
+    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
+    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
+    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
+    VIcp36_19 = ORcp36_19+VIcp36_17;
+    VIcp36_29 = ORcp36_29+VIcp36_27;
+    VIcp36_39 = ORcp36_39+VIcp36_37;
+    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
+    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
+    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
+    POcp36_110 = POcp36_19+RLcp36_110;
+    POcp36_210 = POcp36_29+RLcp36_210;
+    POcp36_310 = POcp36_39+RLcp36_310;
+    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
+    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
+    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
+    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
+    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
+    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
+    VIcp36_110 = ORcp36_110+VIcp36_19;
+    VIcp36_210 = ORcp36_210+VIcp36_29;
+    VIcp36_310 = ORcp36_310+VIcp36_39;
+    RLcp36_111 = ROcp36_421*dpt[2][52];
+    RLcp36_211 = ROcp36_521*dpt[2][52];
+    RLcp36_311 = ROcp36_621*dpt[2][52];
+    POcp36_111 = POcp36_110+RLcp36_111;
+    POcp36_211 = POcp36_210+RLcp36_211;
+    POcp36_311 = POcp36_310+RLcp36_311;
+    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
+    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
+    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
+    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
+    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
+    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
+    VIcp36_111 = ORcp36_111+VIcp36_110;
+    VIcp36_211 = ORcp36_211+VIcp36_210;
+    VIcp36_311 = ORcp36_311+VIcp36_310;
+    RLcp36_112 = ROcp36_727*dpt[3][54];
+    RLcp36_212 = ROcp36_827*dpt[3][54];
+    RLcp36_312 = ROcp36_927*dpt[3][54];
+    POcp36_112 = POcp36_111+RLcp36_112;
+    POcp36_212 = POcp36_211+RLcp36_212;
+    POcp36_312 = POcp36_311+RLcp36_312;
+    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
+    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
+    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
+    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
+    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
+    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
+    VIcp36_112 = ORcp36_112+VIcp36_111;
+    VIcp36_212 = ORcp36_212+VIcp36_211;
+    VIcp36_312 = ORcp36_312+VIcp36_311;
+    RLcp36_113 = ROcp36_727*dpt[3][57];
+    RLcp36_213 = ROcp36_827*dpt[3][57];
+    RLcp36_313 = ROcp36_927*dpt[3][57];
+    POcp36_113 = POcp36_112+RLcp36_113;
+    POcp36_213 = POcp36_212+RLcp36_213;
+    POcp36_313 = POcp36_312+RLcp36_313;
+    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
+    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
+    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
+    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
+    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
+    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
+    VIcp36_113 = ORcp36_113+VIcp36_112;
+    VIcp36_213 = ORcp36_213+VIcp36_212;
+    VIcp36_313 = ORcp36_313+VIcp36_312;
+    PxF12[1] = POcp36_113;
+    PxF12[2] = POcp36_213;
+    PxF12[3] = POcp36_313;
+    RxF12[1][1] = ROcp36_129;
+    RxF12[1][2] = ROcp36_229;
+    RxF12[1][3] = ROcp36_329;
+    RxF12[2][1] = ROcp36_428;
+    RxF12[2][2] = ROcp36_528;
+    RxF12[2][3] = ROcp36_628;
+    RxF12[3][1] = ROcp36_729;
+    RxF12[3][2] = ROcp36_829;
+    RxF12[3][3] = ROcp36_929;
+    VxF12[1] = VIcp36_113;
+    VxF12[2] = VIcp36_213;
+    VxF12[3] = VIcp36_313;
+    OMxF12[1] = OMcp36_113;
+    OMxF12[2] = OMcp36_213;
+    OMxF12[3] = OMcp36_313;
+ 
+    // output copy
+    for(i=1; i<=3; i++)
+    {
+        PxF_tab[0][i] = PxF1[i];
+        PxF_tab[1][i] = PxF2[i];
+        PxF_tab[2][i] = PxF3[i];
+        PxF_tab[3][i] = PxF4[i];
+        PxF_tab[4][i] = PxF5[i];
+        PxF_tab[5][i] = PxF6[i];
+        PxF_tab[6][i] = PxF7[i];
+        PxF_tab[7][i] = PxF8[i];
+        PxF_tab[8][i] = PxF9[i];
+        PxF_tab[9][i] = PxF10[i];
+        PxF_tab[10][i] = PxF11[i];
+        PxF_tab[11][i] = PxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        for(j=1; j<=3; j++)
+        {
+        RxF_tab[0][i][j] = RxF1[i][j];
+        RxF_tab[1][i][j] = RxF2[i][j];
+        RxF_tab[2][i][j] = RxF3[i][j];
+        RxF_tab[3][i][j] = RxF4[i][j];
+        RxF_tab[4][i][j] = RxF5[i][j];
+        RxF_tab[5][i][j] = RxF6[i][j];
+        RxF_tab[6][i][j] = RxF7[i][j];
+        RxF_tab[7][i][j] = RxF8[i][j];
+        RxF_tab[8][i][j] = RxF9[i][j];
+        RxF_tab[9][i][j] = RxF10[i][j];
+        RxF_tab[10][i][j] = RxF11[i][j];
+        RxF_tab[11][i][j] = RxF12[i][j];
+        }
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        VxF_tab[0][i] = VxF1[i];
+        VxF_tab[1][i] = VxF2[i];
+        VxF_tab[2][i] = VxF3[i];
+        VxF_tab[3][i] = VxF4[i];
+        VxF_tab[4][i] = VxF5[i];
+        VxF_tab[5][i] = VxF6[i];
+        VxF_tab[6][i] = VxF7[i];
+        VxF_tab[7][i] = VxF8[i];
+        VxF_tab[8][i] = VxF9[i];
+        VxF_tab[9][i] = VxF10[i];
+        VxF_tab[10][i] = VxF11[i];
+        VxF_tab[11][i] = VxF12[i];
+    }
+
+    for(i=1; i<=3; i++)
+    {
+        OMxF_tab[0][i] = OMxF1[i];
+        OMxF_tab[1][i] = OMxF2[i];
+        OMxF_tab[2][i] = OMxF3[i];
+        OMxF_tab[3][i] = OMxF4[i];
+        OMxF_tab[4][i] = OMxF5[i];
+        OMxF_tab[5][i] = OMxF6[i];
+        OMxF_tab[6][i] = OMxF7[i];
+        OMxF_tab[7][i] = OMxF8[i];
+        OMxF_tab[8][i] = OMxF9[i];
+        OMxF_tab[9][i] = OMxF10[i];
+        OMxF_tab[10][i] = OMxF11[i];
+        OMxF_tab[11][i] = OMxF12[i];
+    }
+}
diff --git a/dataR/extra/short_feet_simp/simp_mbs.txt b/dataR/extra/short_feet_simp/simp_mbs.txt
new file mode 100644
index 0000000..66e76ed
--- /dev/null
+++ b/dataR/extra/short_feet_simp/simp_mbs.txt
@@ -0,0 +1,139 @@
+====== Waist + DWL ======
+--- mass ---
+2.55485
+
+--- COM ---
+-0.013245
+0.00058051
+0.0719568
+
+--- Inertia ---
+0.00773126 -9.327e-05 0.000141837 
+-9.327e-05 0.00997082 -0.000118436 
+0.000141837 -0.000118436 0.00388911 
+
+====== RHipmot + RThighUpLeg + RThighLowLeg ======
+--- mass ---
+3.61731
+
+--- COM ---
+0.000739959
+-0.0476521
+-0.100464
+
+--- Inertia ---
+0.0271576 3.52594e-05 0.000132356 
+3.52594e-05 0.0265146 -0.00144774 
+0.000132356 -0.00144774 0.00298617 
+
+====== RLowLeg + RFootmot ======
+--- mass ---
+2.13974
+
+--- COM ---
+0.00192208
+-0.0038132
+-0.125144
+
+--- Inertia ---
+0.010726 2.21971e-05 -7.47347e-05 
+2.21971e-05 0.0107086 0.000761197 
+-7.47347e-05 0.000761197 0.00161252 
+
+====== RFoot ======
+--- mass ---
+0.489756
+
+--- COM ---
+0.01
+0
+-0.095
+
+--- Inertia ---
+0.00033467 0 0 
+0 0.00080402 0 
+0 0 0.0011 
+
+====== LHipmot + LThighUpLeg + LThighLowLeg ======
+--- mass ---
+3.61731
+
+--- COM ---
+0.000739959
+0.0476521
+-0.100464
+
+--- Inertia ---
+0.0271576 -2.80405e-05 0.000132356 
+-2.80405e-05 0.0265146 0.0015218 
+0.000132356 0.0015218 0.00298617 
+
+====== LLowLeg + LFootmot ======
+--- mass ---
+2.13974
+
+--- COM ---
+0.00192208
+0.0038132
+-0.125144
+
+--- Inertia ---
+0.010726 -3.33531e-06 -7.47347e-05 
+-3.33531e-06 0.0107086 -0.000238516 
+-7.47347e-05 -0.000238516 0.00161252 
+
+====== LFoot ======
+--- mass ---
+0.489756
+
+--- COM ---
+0.01
+0
+-0.095
+
+--- Inertia ---
+0.00033467 0 0 
+0 0.00080402 0 
+0 0 0.0011 
+
+====== DWS + DWYTorso ======
+--- mass ---
+6.90761
+
+--- COM ---
+-0.032983
+-0.000754861
+0.154622
+
+--- Inertia ---
+0.0438747 0.000178445 0.00240963 
+0.000178445 0.0402919 -0.000643229 
+0.00240963 -0.000643229 0.0266284 
+
+====== RShp + RShr + RShy + RElb ======
+--- mass ---
+3.02396
+
+--- COM ---
+0.00161196
+-0.0675862
+-0.0737184
+
+--- Inertia ---
+0.0199566 0.000107891 5.85641e-05 
+0.000107891 0.019132 -0.00161024 
+0.000246536 -0.00161024 0.00303321 
+
+====== LShp + LShr + LShy + LElb ======
+--- mass ---
+3.02396
+
+--- COM ---
+0.00161196
+0.0675862
+-0.0737184
+
+--- Inertia ---
+0.0199566 0.000148786 5.85641e-05 
+0.000149674 0.019132 0.00214378 
+0.000246536 0.0021437 0.00303321 
diff --git a/dataR/index/flex_feet/specific_simu_index.cc b/dataR/index/flex_feet/specific_simu_index.cc
deleted file mode 100644
index deada52..0000000
--- a/dataR/index/flex_feet/specific_simu_index.cc
+++ /dev/null
@@ -1,259 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-		case SimuJointIndex::RightFlexProx  : return RFlex_pr_T3_id;
-		case SimuJointIndex::RightFlexDist  : return RFlex_ds_T3_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-		case SimuJointIndex::LeftFlexProx  : return LFlex_pr_T3_id;
-		case SimuJointIndex::LeftFlexDist  : return LFlex_ds_T3_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-	
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-		case SimuBodyIndex::RFlexPr  : return RFlexPr_id;
-		case SimuBodyIndex::RFlexDs  : return RFlexDs_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-		case SimuBodyIndex::LFlexPr  : return LFlexPr_id;
-		case SimuBodyIndex::LFlexDs  : return LFlexDs_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// upper feet part
-		case SimuSsensIndex::RightFoot : return RFoot_Upper_id;
-		case SimuSsensIndex::LeftFoot  : return LFoot_Upper_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFlexProx : return mbs_data->Nsensor + RFlex_pr_force_id;
-		case SimuSsensIndex::RightFlexDist : return mbs_data->Nsensor + RFlex_ds_force_id;
-		case SimuSsensIndex::LeftFlexProx  : return mbs_data->Nsensor + LFlex_pr_force_id;
-		case SimuSsensIndex::LeftFlexDist  : return mbs_data->Nsensor + LFlex_ds_force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFlexProx : return RFlex_pr_force_id;
-		case SimuFsensIndex::RightFlexDist : return RFlex_ds_force_id;
-		case SimuFsensIndex::LeftFlexProx  : return LFlex_pr_force_id;
-		case SimuFsensIndex::LeftFlexDist  : return LFlex_ds_force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	switch (link_id)
-	{
-		case SimuLinkIndex::RightFlexProx : return Link_RFlexProx_id;
-		case SimuLinkIndex::RightFlexDist : return Link_RFlexDist_id;
-		case SimuLinkIndex::LeftFlexProx  : return Link_LFlexProx_id;
-		case SimuLinkIndex::LeftFlexDist  : return Link_LFlexDist_id;
-	
-		default:
-			std::cout << "Error: unknown link index: " << link_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
diff --git a/dataR/index/init_feet/specific_simu_index.cc b/dataR/index/init_feet/specific_simu_index.cc
deleted file mode 100644
index 40fc295..0000000
--- a/dataR/index/init_feet/specific_simu_index.cc
+++ /dev/null
@@ -1,234 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-	
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
-		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
-		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
-	exit(EXIT_FAILURE);
-
-	return -1;
-}
diff --git a/dataR/index/long_feet/specific_simu_index.cc b/dataR/index/long_feet/specific_simu_index.cc
deleted file mode 100644
index 40fc295..0000000
--- a/dataR/index/long_feet/specific_simu_index.cc
+++ /dev/null
@@ -1,234 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-	
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
-		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
-		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
-	exit(EXIT_FAILURE);
-
-	return -1;
-}
diff --git a/dataR/index/short_feet/specific_simu_index.cc b/dataR/index/short_feet/specific_simu_index.cc
deleted file mode 100644
index 19549c8..0000000
--- a/dataR/index/short_feet/specific_simu_index.cc
+++ /dev/null
@@ -1,236 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \param[in] mbs_data Robotran structure
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
-		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
-		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
-	exit(EXIT_FAILURE);
-
-	return -1;
-}
diff --git a/dataR/index/short_feet_ball/specific_simu_index.cc b/dataR/index/short_feet_ball/specific_simu_index.cc
deleted file mode 100644
index cb188dc..0000000
--- a/dataR/index/short_feet_ball/specific_simu_index.cc
+++ /dev/null
@@ -1,252 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-
-		// ball A
-		case SimuJointIndex::Ball_A_T1 : return FJ_T1_Ball_id;
-		case SimuJointIndex::Ball_A_T2 : return FJ_T2_Ball_id;
-		case SimuJointIndex::Ball_A_T3 : return FJ_T3_Ball_id;
-		case SimuJointIndex::Ball_A_R1 : return FJ_R1_Ball_id;
-		case SimuJointIndex::Ball_A_R2 : return FJ_R2_Ball_id;
-		case SimuJointIndex::Ball_A_R3 : return FJ_R3_Ball_id;
-
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-
-		case SimuBodyIndex::Ball_A : return Ball_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \param[in] mbs_data Robotran structure
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
-		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
-
-		// balls
-		case SimuSsensIndex::Ball_A : return mbs_data->Nsensor + Ball_Force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
-		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
-
-		// balls
-		case SimuFsensIndex::Ball_A : return Ball_Force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
-	exit(EXIT_FAILURE);
-
-	return -1;
-}
diff --git a/dataR/index/short_feet_contact/specific_simu_index.cc b/dataR/index/short_feet_contact/specific_simu_index.cc
deleted file mode 100644
index 19549c8..0000000
--- a/dataR/index/short_feet_contact/specific_simu_index.cc
+++ /dev/null
@@ -1,236 +0,0 @@
-#include "ModelSimuIndex.hh"
-#include "SimuIndex.hh"
-#include "user_all_id.h"
-
-#include <iostream>
-#include <stdlib.h>
-
-/*! \brief get the indexes related to the MBS file for the joints
- * 
- * \param[in] joint_id ID of the requested joint
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsJointId(int joint_id)
-{
-	switch (joint_id)
-	{
-		// floating base
-		case SimuJointIndex::FloatingT1 : return FJ_T1_Coman_id;
-		case SimuJointIndex::FloatingT2 : return FJ_T2_Coman_id;
-		case SimuJointIndex::FloatingT3 : return FJ_T3_Coman_id;
-		case SimuJointIndex::FloatingR1 : return FJ_R1_Coman_id;
-		case SimuJointIndex::FloatingR2 : return FJ_R2_Coman_id;
-		case SimuJointIndex::FloatingR3 : return FJ_R3_Coman_id;
-
-		// right leg
-		case SimuJointIndex::RightHipPitch  : return RHipSag_id;
-		case SimuJointIndex::RightHipRoll   : return RHipLat_id;
-		case SimuJointIndex::RightHipYaw    : return RHipYaw_id;
-		case SimuJointIndex::RightKneePitch : return RKneeSag_id;
-		case SimuJointIndex::RightFootRoll  : return RAnkLat_id;
-		case SimuJointIndex::RightFootPitch : return RAnkSag_id;
-
-		// left leg
-		case SimuJointIndex::LeftHipPitch  : return LHipSag_id;
-		case SimuJointIndex::LeftHipRoll   : return LHipLat_id;
-		case SimuJointIndex::LeftHipYaw    : return LHipYaw_id;
-		case SimuJointIndex::LeftKneePitch : return LKneeSag_id;
-		case SimuJointIndex::LeftFootRoll  : return LAnkLat_id;
-		case SimuJointIndex::LeftFootPitch : return LAnkSag_id;
-
-		// torso
-		case SimuJointIndex::TorsoRoll  : return WaistLat_id;
-		case SimuJointIndex::TorsoPitch : return WaistSag_id;
-		case SimuJointIndex::TorsoYaw   : return WaistYaw_id;
-
-		// right arm
-		case SimuJointIndex::RightShPitch  : return RShSag_id;
-		case SimuJointIndex::RightShRoll   : return RShLat_id;
-		case SimuJointIndex::RightShYaw    : return RShYaw_id;
-		case SimuJointIndex::RightElbPitch : return RElb_id;
-
-		// left arm
-		case SimuJointIndex::LeftShPitch  : return LShSag_id;
-		case SimuJointIndex::LeftShRoll   : return LShLat_id;
-		case SimuJointIndex::LeftShYaw    : return LShYaw_id;
-		case SimuJointIndex::LeftElbPitch : return LElb_id;
-
-		default:
-			std::cout << "Error: unknown joint index: " << joint_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the bodies
- * 
- * \param[in] body_id ID of the requested body
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsBodyId(int body_id)
-{
-	switch (body_id)
-	{
-		case SimuBodyIndex::Waist : return Waist_id;
-
-		case SimuBodyIndex::RHipmot : return RHipmot_id;
-		case SimuBodyIndex::RThighUpLeg  : return RThighUpLeg_id;
-		case SimuBodyIndex::RThighLowLeg : return RThighLowLeg_id;
-		case SimuBodyIndex::RLowLeg  : return RLowLeg_id;
-		case SimuBodyIndex::RFootmot : return RFootmot_id;
-		case SimuBodyIndex::RFoot    : return RFoot_id;
-
-		case SimuBodyIndex::LHipmot : return LHipmot_id;
-		case SimuBodyIndex::LThighUpLeg  : return LThighUpLeg_id;
-		case SimuBodyIndex::LThighLowLeg : return LThighLowLeg_id;
-		case SimuBodyIndex::LLowLeg  : return LLowLeg_id;
-		case SimuBodyIndex::LFootmot : return LFootmot_id;
-		case SimuBodyIndex::LFoot    : return LFoot_id;
-
-		case SimuBodyIndex::DWL : return DWL_id;
-		case SimuBodyIndex::DWS : return DWS_id;
-		case SimuBodyIndex::DWYTorso : return DWYTorso_id;
-
-		case SimuBodyIndex::RShp : return RShp_id;
-		case SimuBodyIndex::RShr : return RShr_id;
-		case SimuBodyIndex::RShy : return RShy_id;
-		case SimuBodyIndex::RElb : return RElb_id;
-
-		case SimuBodyIndex::LShp : return LShp_id;
-		case SimuBodyIndex::LShr : return LShr_id;
-		case SimuBodyIndex::LShy : return LShy_id;
-		case SimuBodyIndex::LElb : return LElb_id;
-	
-		default:
-				std::cout << "Error: unknown body index: " << body_id << std::endl;
-				exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the S sensors
- * 
- * \param[in] Ssens_id ID of the requested S sensor
- * \param[in] mbs_data Robotran structure
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsSSensId(int Ssens_id)
-{
-	switch (Ssens_id)
-	{
-		// mid-waist
-		case SimuSsensIndex::MidWaist : return mbs_data->Nsensor + Waist_contact_force_id;
-
-		// one part feet
-		case SimuSsensIndex::RightFoot : return mbs_data->Nsensor + RFoot_Force_id;
-		case SimuSsensIndex::LeftFoot  : return mbs_data->Nsensor + LFoot_Force_id;
-
-		// legs
-		case SimuSsensIndex::RThigh  : return mbs_data->Nsensor + RThigh_contact_force_id;
-		case SimuSsensIndex::LThigh  : return mbs_data->Nsensor + LThigh_contact_force_id;
-		case SimuSsensIndex::RLowLeg : return mbs_data->Nsensor + RLowLeg_contact_force_id;
-		case SimuSsensIndex::LLowLeg : return mbs_data->Nsensor + LLowLeg_contact_visu_id;
-
-		// torso
-		case SimuSsensIndex::Torso : return mbs_data->Nsensor + Torso_contact_force_id;
-
-		// arms
-		case SimuSsensIndex::RShoulder : return mbs_data->Nsensor + RShoulder_contact_force_id;
-		case SimuSsensIndex::LShoulder : return mbs_data->Nsensor + LShoulder_contact_force_id;
-		case SimuSsensIndex::RElbow    : return mbs_data->Nsensor + RElbow_contact_force_id;
-		case SimuSsensIndex::LElbow    : return mbs_data->Nsensor + LElbow_contact_force_id;
-
-		// com
-		case SimuSsensIndex::WaistCg : return WaistCg_id;
-
-		case SimuSsensIndex::RHipmotCg      : return RHipmotCg_id;
-		case SimuSsensIndex::RThighUpLegCg  : return RThighUpLegCg_id;
-		case SimuSsensIndex::RThighLowLegCg : return RThighLowLegCg_id;
-
-		case SimuSsensIndex::RLowLegCg  : return RLowLegCg_id;
-		case SimuSsensIndex::RFootmotCg : return RFootmotCg_id;
-		case SimuSsensIndex::RFootCg    : return RFootCg_id;
-
-		case SimuSsensIndex::LHipmotCg      : return LHipmotCg_id;
-		case SimuSsensIndex::LThighUpLegCg  : return LThighUpLegCg_id;
-		case SimuSsensIndex::LThighLowLegCg : return LThighLowLegCg_id;
-
-		case SimuSsensIndex::LLowLegCg  : return LLowLegCg_id;
-		case SimuSsensIndex::LFootmotCg : return LFootmotCg_id;
-		case SimuSsensIndex::LFootCg    : return LFootCg_id;
-
-		case SimuSsensIndex::DWLCg : return DWLCg_id;
-		case SimuSsensIndex::DWSCg : return DWSCg_id;
-		case SimuSsensIndex::DWYCg : return DWYCg_id;
-
-		case SimuSsensIndex::RShpCg : return RShpCg_id;
-		case SimuSsensIndex::RShrCg : return RShrCg_id;
-		case SimuSsensIndex::RShyCg : return RShyCg_id;
-		case SimuSsensIndex::RElbCg : return RElbCg_id;
-
-		case SimuSsensIndex::LShpCg : return LShpCg_id;
-		case SimuSsensIndex::LShrCg : return LShrCg_id;
-		case SimuSsensIndex::LShyCg : return LShyCg_id;
-		case SimuSsensIndex::LElbCg : return LElbCg_id;
-	
-		default:
-			std::cout << "Error: unknown S sensor index: " << Ssens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the F sensors
- * 
- * \param[in] Fsens_id ID of the requested F sensor
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsFSensId(int Fsens_id)
-{
-	switch (Fsens_id)
-	{
-		// mid-waist
-		case SimuFsensIndex::MidWaist : return Waist_contact_force_id;
-
-		// legs
-		case SimuFsensIndex::RThigh  : return RThigh_contact_force_id;
-		case SimuFsensIndex::LThigh  : return LThigh_contact_force_id;
-		case SimuFsensIndex::RLowLeg : return RLowLeg_contact_force_id;
-		case SimuFsensIndex::LLowLeg : return LLowLeg_contact_visu_id;
-
-		// one part feet
-		case SimuFsensIndex::RightFoot : return RFoot_Force_id;
-		case SimuFsensIndex::LeftFoot  : return LFoot_Force_id;
-
-		// torso
-		case SimuFsensIndex::Torso : return Torso_contact_force_id;
-
-		// arms
-		case SimuFsensIndex::RShoulder : return RShoulder_contact_force_id;
-		case SimuFsensIndex::LShoulder : return LShoulder_contact_force_id;
-		case SimuFsensIndex::RElbow    : return RElbow_contact_force_id;
-		case SimuFsensIndex::LElbow    : return LElbow_contact_force_id;
-	
-		default:
-			std::cout << "Error: unknown F sensor index: " << Fsens_id << std::endl;
-			exit(EXIT_FAILURE);
-	}
-
-	return -1;
-}
-
-/*! \brief get the indexes related to the MBS file for the links
- * 
- * \param[in] link_id ID of the requested link
- * \return requested MBS index
- */
-int ModelSimuIndex::ExtractMbsLinkId(int link_id)
-{
-	std::cout << "Error: this COMAN model does not feature any link !" << std::endl;
-	exit(EXIT_FAILURE);
-
-	return -1;
-}
diff --git a/dataR/visu/coman_short_feet_visu_obj.mbs b/dataR/visu/coman_short_feet_visu_obj.mbs
index bc1c281..698ba7b 100644
--- a/dataR/visu/coman_short_feet_visu_obj.mbs
+++ b/dataR/visu/coman_short_feet_visu_obj.mbs
@@ -1,3941 +1,3906 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<mbsDescR7>
-  <mbsname>coman_short_feet</mbsname>
-  <mbsyspad_version>1.9.4</mbsyspad_version>
-  <base>
-    <!--this is the base body-->
-    <gravity>
-      <coordinates>
-        <x>0.0</x>
-        <y>0.0</y>
-        <z>-9.81</z>
-      </coordinates>
-    </gravity>
-    <baseBodyName>BaseBody</baseBodyName>
-    <graphics>
-      <x2D>
-        <position>
-          <x>220.0</x>
-          <y>1180.0</y>
-        </position>
-        <base_body>
-          <position>
-            <x>-28.0</x>
-            <y>-66.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>126.0</x>
-            <y>89.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>98.0</x>
-              <y>-66.0</y>
-            </control_point>
-            <control_point>
-              <x>98.0</x>
-              <y>23.0</y>
-            </control_point>
-            <control_point>
-              <x>-28.0</x>
-              <y>23.0</y>
-            </control_point>
-            <control_point>
-              <x>-28.0</x>
-              <y>-66.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </base_body>
-      </x2D>
-      <x3D>
-        <x3D_frame />
-        <x3D_shapes>
-          <inline>
-            <name>Chess_ground</name>
-            <position>
-              <x>0.0</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </position>
-            <scale>
-              <x>1.0</x>
-              <y>1.0</y>
-              <z>1.0</z>
-            </scale>
-            <rotation>
-              <x>1.5707963267948966</x>
-              <y>0.0</y>
-              <z>0.0</z>
-            </rotation>
-            <color R="0" G="0" B="0" />
-            <transparency>0.0</transparency>
-            <url>PRJPATH/../../animationR/obj/ground/chess_ground.obj</url>
-          </inline>
-        </x3D_shapes>
-      </x3D>
-    </graphics>
-  </base>
-  <bodytree>
-    <!--this is the body tree-->
-    <body>
-      <bodyname>CameraPoint</bodyname>
-      <parent>
-        <bodyname>base</bodyname>
-        <pointname>origin</pointname>
-      </parent>
-      <joint>
-        <jointname>FJ_T1_Coman</jointname>
-        <type>T1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>264.0</x>
-              <y>1180.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T2_Coman</jointname>
-        <type>T2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>306.0</x>
-              <y>1181.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T3_Coman</jointname>
-        <type>T3</type>
-        <nature>independent</nature>
-        <initialvalue>
-          <q>0.5268</q>
-        </initialvalue>
-        <graphics>
-          <x2D>
-            <position>
-              <x>352.0</x>
-              <y>1181.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0</mass>
-      <inertia>
-        <Ixx>0.001</Ixx>
-        <Iyy>0.001</Iyy>
-        <Izz>0.001</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_Camera</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>352.0</x>
-              <y>1215.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>3.0</x>
-            <y>-12.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>34.0</x>
-            <y>34.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>37.0</x>
-              <y>-12.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D />
-      </graphics>
-    </body>
-    <body>
-      <bodyname>Waist</bodyname>
-      <parent>
-        <bodyname>CameraPoint</bodyname>
-        <pointname>origin</pointname>
-      </parent>
-      <joint>
-        <jointname>FJ_R1_Coman</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>317.0</x>
-              <y>770.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R2_Coman</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>304.0</x>
-              <y>627.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R3_Coman</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>299.0</x>
-              <y>472.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.8008695</mass>
-      <com>
-        <coordinates>
-          <x>-0.026391145</x>
-          <y>-5.8216364E-4</y>
-          <z>0.052632312</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0051032982</Ixx>
-        <Ixz>0.0016920543</Ixz>
-        <Iyy>0.006241636</Iyy>
-        <Iyz>1.7225949E-5</Iyz>
-        <Izz>0.0024517762</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="Waist_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_Waist_1</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.023</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>217.0</x>
-              <y>438.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_2</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.023</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>404.0</x>
-              <y>445.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_3</pointname>
-        <coordinates>
-          <x>0.020281504</x>
-          <y>0.0</y>
-          <z>0.11912053</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>306.0</x>
-              <y>395.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_Waist_Cg</pointname>
-        <coordinates>
-          <x>-0.026391145</x>
-          <y>-5.8216364E-4</y>
-          <z>0.052632312</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>362.0</x>
-              <y>419.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="WaistCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-83.0</x>
-            <y>-75.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>190.0</x>
-            <y>96.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>107.0</x>
-              <y>-75.0</y>
-            </control_point>
-            <control_point>
-              <x>107.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>-83.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>-83.0</x>
-              <y>-75.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Waist_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_07_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RHipmot</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_1</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>164.0</x>
-              <y>435.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.89258227</mass>
-      <com>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>-0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.7672294E-4</Ixx>
-        <Iyy>5.458038E-4</Iyy>
-        <Izz>5.126859E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RHipmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.0496</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>105.0</x>
-              <y>434.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RHipmot_Cg</pointname>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>-0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>125.0</x>
-              <y>399.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RHipmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-31.0</x>
-            <y>1.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>34.0</x>
-            <y>34.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>-9.0</x>
-              <y>-3.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RHip_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.05</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_20_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RThighUpLeg</bodyname>
-      <parent>
-        <bodyname>RHipmot</bodyname>
-        <pointname>Point_RHipmot</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>97.0</x>
-              <y>506.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0246101</mass>
-      <com>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>-0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.165719E-4</Ixx>
-        <Iyy>0.0010484958</Iyy>
-        <Iyz>2.5492874E-5</Iyz>
-        <Izz>6.8637197E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RThighUpLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1024</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>97.0</x>
-              <y>609.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RThighUpLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>-0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>110.0</x>
-              <y>563.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RThighUpLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-9.0</x>
-            <y>5.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>25.0</x>
-            <y>101.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>28.0</x>
-              <y>-1.0</y>
-            </control_point>
-            <control_point>
-              <x>28.0</x>
-              <y>61.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>61.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>-1.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RThighUp_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_19_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RThighLowLeg</bodyname>
-      <parent>
-        <bodyname>RThighUpLeg</bodyname>
-        <pointname>Point_RThighUpLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RHipYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>670.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.7001134</mass>
-      <com>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0038979546</Ixx>
-        <Ixy>1.7802399E-6</Ixy>
-        <Ixz>2.6403262E-5</Ixz>
-        <Iyy>0.0039360845</Iyy>
-        <Iyz>4.522711E-5</Iyz>
-        <Izz>9.0218711E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RThighLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1234</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>752.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RThighLowLeg_Cg</pointname>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>109.0</x>
-              <y>719.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RThighLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>83.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RThighLow_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.42</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_18_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RLowLeg</bodyname>
-      <parent>
-        <bodyname>RThighLowLeg</bodyname>
-        <pointname>Point_RThighLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RKneeSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>91.0</x>
-              <y>816.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.4098179</mass>
-      <com>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>-0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0040604004</Ixx>
-        <Ixy>1.1227206E-5</Ixy>
-        <Ixz>1.2260973E-5</Ixz>
-        <Iyy>0.0040056528</Iyy>
-        <Iyz>5.1899176E-4</Iyz>
-        <Izz>0.0012414054</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RLowLeg_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.201</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>90.0</x>
-              <y>911.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RLowLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>-0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>104.0</x>
-              <y>868.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-13.0</x>
-            <y>3.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>95.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>24.0</x>
-              <y>3.0</y>
-            </control_point>
-            <control_point>
-              <x>24.0</x>
-              <y>91.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>91.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>3.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RLowLeg_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.3</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_17_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RFootmot</bodyname>
-      <parent>
-        <bodyname>RLowLeg</bodyname>
-        <pointname>Point_RLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>RAnkLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>94.0</x>
-              <y>982.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.72992131</mass>
-      <com>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>-9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.2034425E-4</Ixx>
-        <Ixy>7.6346303E-6</Ixy>
-        <Ixz>2.5430158E-7</Ixz>
-        <Iyy>3.6584358E-4</Iyy>
-        <Izz>3.6065156E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RFootmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>91.0</x>
-              <y>1026.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_RFootmot_Cg</pointname>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>-9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>116.0</x>
-              <y>1008.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RFootmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>0.0</x>
-            <y>23.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>25.0</x>
-            <y>25.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>25.0</x>
-              <y>23.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RFootMot_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>0.08</y>
-                <z>-0.1</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_16_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RFoot</bodyname>
-      <parent>
-        <bodyname>RFootmot</bodyname>
-        <pointname>Point_RFootmot</pointname>
-      </parent>
-      <joint>
-        <jointname>RAnkSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>84.0</x>
-              <y>1120.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.489756</mass>
-      <com>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.3467E-4</Ixx>
-        <Iyy>8.0402E-4</Iyy>
-        <Izz>0.0011</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RFoot_Cg</pointname>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>109.0</x>
-              <y>1108.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RFootCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RFoot_F</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>85.0</x>
-              <y>1194.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>-20.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>77.0</x>
-            <y>112.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>41.0</x>
-              <y>-20.0</y>
-            </control_point>
-            <control_point>
-              <x>41.0</x>
-              <y>92.0</y>
-            </control_point>
-            <control_point>
-              <x>-36.0</x>
-              <y>92.0</y>
-            </control_point>
-            <control_point>
-              <x>-36.0</x>
-              <y>-20.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Cache_left</name>
-              <position>
-                <x>0.0</x>
-                <y>0.041</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
-            </inline>
-            <inline>
-              <name>Cache_right</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.036</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
-            </inline>
-            <box>
-              <name>Bar_right</name>
-              <position>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>-0.06</z>
-              </position>
-              <scale>
-                <x>0.035</x>
-                <y>0.07</y>
-                <z>0.05</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="150" G="150" B="150" />
-              <transparency>0.0</transparency>
-              <size>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </size>
-            </box>
-            <inline>
-              <name>Right_foot</name>
-              <position>
-                <x>-0.06</x>
-                <y>-0.0525</y>
-                <z>-0.0795</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>-1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/short/coman_short_foot.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LHipmot</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_2</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>467.0</x>
-              <y>443.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.89258227</mass>
-      <com>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.7672294E-4</Ixx>
-        <Ixy>3.9936567E-6</Ixy>
-        <Iyy>5.458038E-4</Iyy>
-        <Iyz>3.3389575E-6</Iyz>
-        <Izz>5.126859E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LHipmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0496</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>534.0</x>
-              <y>444.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LHipmot_Cg</pointname>
-        <coordinates>
-          <x>-3.269678E-4</x>
-          <y>0.023587321</y>
-          <z>-2.6496509E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>503.0</x>
-              <y>409.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LHipmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>36.0</x>
-            <y>0.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>34.0</x>
-            <y>34.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>63.0</x>
-              <y>-10.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LHip_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.04</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_06_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LThighUpLeg</bodyname>
-      <parent>
-        <bodyname>LHipmot</bodyname>
-        <pointname>Point_LHipmot</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>519.0</x>
-              <y>520.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0246101</mass>
-      <com>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.165719E-4</Ixx>
-        <Ixy>1.4449904E-6</Ixy>
-        <Iyy>0.0010484958</Iyy>
-        <Izz>6.8637197E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LThighUpLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1024</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>519.0</x>
-              <y>623.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LThighUpLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0012591632</x>
-          <y>0.016896372</y>
-          <z>-0.058903336</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>488.0</x>
-              <y>578.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LThighUpLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>7.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>24.0</x>
-            <y>101.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>12.0</x>
-              <y>7.0</y>
-            </control_point>
-            <control_point>
-              <x>12.0</x>
-              <y>108.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>108.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>7.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LThighUp_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.52</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_05_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LThighLowLeg</bodyname>
-      <parent>
-        <bodyname>LThighUpLeg</bodyname>
-        <pointname>Point_LThighUpLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LHipYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>521.0</x>
-              <y>680.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.7001134</mass>
-      <com>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>-6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0038979546</Ixx>
-        <Ixz>2.6403262E-5</Ixz>
-        <Iyy>0.0039360845</Iyy>
-        <Izz>9.0218711E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LThighLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1234</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>523.0</x>
-              <y>759.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LThighLowLeg_Cg</pointname>
-        <coordinates>
-          <x>9.8720058E-4</x>
-          <y>-6.7037262E-4</y>
-          <z>-0.075716843</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>509.0</x>
-              <y>730.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LThighLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>25.0</x>
-            <y>82.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>13.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>13.0</x>
-              <y>86.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>86.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LTighLow_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.078</y>
-                <z>-0.42</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_04_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LLowLeg</bodyname>
-      <parent>
-        <bodyname>LThighLowLeg</bodyname>
-        <pointname>Point_LThighLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LKneeSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>516.0</x>
-              <y>824.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.4098179</mass>
-      <com>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0040604004</Ixx>
-        <Ixz>1.2260973E-5</Ixz>
-        <Iyy>0.0040056528</Iyy>
-        <Izz>0.0012414054</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LLowLeg_contact_visu" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LLowLeg</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.201</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>517.0</x>
-              <y>916.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LLowLeg_Cg</pointname>
-        <coordinates>
-          <x>0.0024612666</x>
-          <y>0.0053099614</y>
-          <z>-0.085989484</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>493.0</x>
-              <y>890.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LLowLegCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-15.0</x>
-            <y>10.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>28.0</x>
-            <y>88.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>29.0</x>
-              <y>-17.0</y>
-            </control_point>
-            <control_point>
-              <x>29.0</x>
-              <y>52.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>52.0</y>
-            </control_point>
-            <control_point>
-              <x>4.0</x>
-              <y>-17.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LLowLeg_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.3</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_03_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LFootmot</bodyname>
-      <parent>
-        <bodyname>LLowLeg</bodyname>
-        <pointname>Point_LLowLeg</pointname>
-      </parent>
-      <joint>
-        <jointname>LAnkLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>512.0</x>
-              <y>985.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.72992131</mass>
-      <com>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.2034425E-4</Ixx>
-        <Ixz>2.5430158E-7</Ixz>
-        <Iyy>3.6584358E-4</Iyy>
-        <Iyz>3.6889276E-6</Iyz>
-        <Izz>3.6065156E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LFootmot</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>509.0</x>
-              <y>1030.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_LFootmot_Cg</pointname>
-        <coordinates>
-          <x>8.8067281E-4</x>
-          <y>9.2224881E-4</y>
-          <z>2.3038476E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>488.0</x>
-              <y>1009.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LFootmotCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-4.0</x>
-            <y>23.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>21.0</x>
-            <y>21.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>39.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LFootMot_visu</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.08</y>
-                <z>-0.1</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_02_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LFoot</bodyname>
-      <parent>
-        <bodyname>LFootmot</bodyname>
-        <pointname>Point_LFootmot</pointname>
-      </parent>
-      <joint>
-        <jointname>LAnkSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>495.0</x>
-              <y>1148.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.489756</mass>
-      <com>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.3467E-4</Ixx>
-        <Iyy>8.0402E-4</Iyy>
-        <Izz>0.0011</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LFoot_Cg</pointname>
-        <coordinates>
-          <x>0.01</x>
-          <y>0.0</y>
-          <z>-0.095</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>522.0</x>
-              <y>1144.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LFootCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LFoot_F</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>495.0</x>
-              <y>1221.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-37.0</x>
-            <y>-19.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>72.0</x>
-            <y>108.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>35.0</x>
-              <y>-19.0</y>
-            </control_point>
-            <control_point>
-              <x>35.0</x>
-              <y>89.0</y>
-            </control_point>
-            <control_point>
-              <x>-37.0</x>
-              <y>89.0</y>
-            </control_point>
-            <control_point>
-              <x>-37.0</x>
-              <y>-19.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Cache_left</name>
-              <position>
-                <x>0.0</x>
-                <y>0.041</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
-            </inline>
-            <inline>
-              <name>Cache_right</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.037</y>
-                <z>-0.005</z>
-              </position>
-              <scale>
-                <x>0.85</x>
-                <y>0.85</y>
-                <z>0.85</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
-            </inline>
-            <box>
-              <name>Bar_left</name>
-              <position>
-                <x>0.0</x>
-                <y>-0.0</y>
-                <z>-0.06</z>
-              </position>
-              <scale>
-                <x>0.035</x>
-                <y>0.07</y>
-                <z>0.05</z>
-              </scale>
-              <rotation>
-                <x>0.0</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="150" G="150" B="150" />
-              <transparency>0.0</transparency>
-              <size>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </size>
-            </box>
-            <inline>
-              <name>Left_foot</name>
-              <position>
-                <x>-0.06</x>
-                <y>-0.0525</y>
-                <z>-0.0795</z>
-              </position>
-              <scale>
-                <x>1.0</x>
-                <y>1.0</y>
-                <z>1.0</z>
-              </scale>
-              <rotation>
-                <x>-1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/feet/short/coman_short_foot.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWL</bodyname>
-      <parent>
-        <bodyname>Waist</bodyname>
-        <pointname>Point_Waist_3</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>305.0</x>
-              <y>331.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.75398402</mass>
-      <com>
-        <coordinates>
-          <x>-0.0021273416</x>
-          <y>0.0033575235</y>
-          <z>-0.0010076896</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>3.4093348E-4</Ixx>
-        <Iyy>3.9581916E-4</Iyy>
-        <Iyz>1.443019E-6</Iyz>
-        <Izz>3.7449753E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_DWL</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>304.0</x>
-              <y>294.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWL_Cg</pointname>
-        <coordinates>
-          <x>-0.0021273416</x>
-          <y>0.0033575235</y>
-          <z>-0.0010076896</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>336.0</x>
-              <y>313.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWLCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-52.0</x>
-            <y>-44.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>103.0</x>
-            <y>55.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>21.0</x>
-              <y>-51.0</y>
-            </control_point>
-            <control_point>
-              <x>21.0</x>
-              <y>-3.0</y>
-            </control_point>
-            <control_point>
-              <x>1.0</x>
-              <y>-3.0</y>
-            </control_point>
-            <control_point>
-              <x>1.0</x>
-              <y>-51.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>DWL1_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.0</y>
-                <z>-0.64</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_09_prt.obj</url>
-            </inline>
-            <inline>
-              <name>DWL2_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.0</y>
-                <z>-0.64</z>
-              </position>
-              <scale>
-                <x>0.039</x>
-                <y>0.039</y>
-                <z>0.039</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_08_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWS</bodyname>
-      <parent>
-        <bodyname>DWL</bodyname>
-        <pointname>Point_DWL</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>310.0</x>
-              <y>245.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.54588774</mass>
-      <com>
-        <coordinates>
-          <x>4.2999707E-4</x>
-          <y>-0.0097650317</y>
-          <z>0.033011157</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>9.7126578E-4</Ixx>
-        <Ixz>1.1440119E-6</Ixz>
-        <Iyy>7.4686706E-4</Iyy>
-        <Izz>6.2196235E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_DWS</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0475</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>308.0</x>
-              <y>192.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWS_Cg</pointname>
-        <coordinates>
-          <x>4.2999707E-4</x>
-          <y>-0.0097650317</y>
-          <z>0.033011157</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>336.0</x>
-              <y>223.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWSCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-35.0</x>
-            <y>-47.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>65.0</x>
-            <y>47.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>0.0</x>
-              <y>-18.0</y>
-            </control_point>
-            <control_point>
-              <x>0.0</x>
-              <y>-6.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-6.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-18.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D />
-      </graphics>
-    </body>
-    <body>
-      <bodyname>DWYTorso</bodyname>
-      <parent>
-        <bodyname>DWS</bodyname>
-        <pointname>Point_DWS</pointname>
-      </parent>
-      <joint>
-        <jointname>WaistYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>307.0</x>
-              <y>144.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>6.3617175</mass>
-      <com>
-        <coordinates>
-          <x>-0.03585012</x>
-          <y>1.8286043E-5</y>
-          <z>0.117557148</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.034089342</Ixx>
-        <Iyy>0.030117357</Iyy>
-        <Iyz>6.2443771E-6</Iyz>
-        <Izz>0.025296582</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="Torso_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_Cg</pointname>
-        <coordinates>
-          <x>-0.03585012</x>
-          <y>1.8286043E-5</y>
-          <z>0.117557148</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>350.0</x>
-              <y>97.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="DWYCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_1</pointname>
-        <coordinates>
-          <x>-0.015</x>
-          <y>-0.0825</y>
-          <z>0.1554</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>252.0</x>
-              <y>114.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <point>
-        <pointname>Point_DWYTorso_2</pointname>
-        <coordinates>
-          <x>-0.015</x>
-          <y>0.0825</y>
-          <z>0.1554</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>375.0</x>
-              <y>114.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-62.0</x>
-            <y>-51.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>138.0</x>
-            <y>61.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>-1.0</x>
-              <y>-8.0</y>
-            </control_point>
-            <control_point>
-              <x>-1.0</x>
-              <y>-5.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-5.0</y>
-            </control_point>
-            <control_point>
-              <x>-2.0</x>
-              <y>-8.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>Torso_visu</name>
-              <position>
-                <x>-0.035</x>
-                <y>0.0</y>
-                <z>-0.69</z>
-              </position>
-              <scale>
-                <x>0.0393</x>
-                <y>0.0393</y>
-                <z>0.0393</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/torso_blue.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShp</bodyname>
-      <parent>
-        <bodyname>DWYTorso</bodyname>
-        <pointname>Point_DWYTorso_1</pointname>
-      </parent>
-      <joint>
-        <jointname>RShSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>217.0</x>
-              <y>105.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.56782202</mass>
-      <com>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>-0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>7.8337456E-4</Ixx>
-        <Ixy>1.9035768E-4</Ixy>
-        <Iyy>6.3399921E-4</Iyy>
-        <Iyz>4.3840774E-6</Iyz>
-        <Izz>0.001008054</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RShp_Cg</pointname>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>-0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>162.0</x>
-              <y>99.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShpCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShp</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>-0.071</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>156.0</x>
-              <y>129.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>7.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>30.0</x>
-            <y>30.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>3.0</x>
-              <y>12.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShp_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.08</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_24_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShr</bodyname>
-      <parent>
-        <bodyname>RShp</bodyname>
-        <pointname>Point_RShp</pointname>
-      </parent>
-      <joint>
-        <jointname>RShLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>114.0</x>
-              <y>123.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.77683752</mass>
-      <com>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.5812904E-4</Ixx>
-        <Ixz>9.2521185E-6</Ixz>
-        <Iyy>4.3089204E-4</Iyy>
-        <Izz>3.4174855E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_RShr_Cg</pointname>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>80.0</x>
-              <y>141.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShrCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShr</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.0425</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>105.0</x>
-              <y>170.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-36.0</x>
-            <y>5.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>56.0</x>
-            <y>43.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>32.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>65.0</y>
-            </control_point>
-            <control_point>
-              <x>-22.0</x>
-              <y>65.0</y>
-            </control_point>
-            <control_point>
-              <x>-22.0</x>
-              <y>21.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShr_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_23_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RShy</bodyname>
-      <parent>
-        <bodyname>RShr</bodyname>
-        <pointname>Point_RShr</pointname>
-      </parent>
-      <joint>
-        <jointname>RShYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>123.0</x>
-              <y>226.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0462304</mass>
-      <com>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>-0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0030934574</Ixx>
-        <Ixy>6.6702837E-5</Ixy>
-        <Ixz>3.0222291E-4</Ixz>
-        <Iyy>0.0029181573</Iyy>
-        <Izz>8.0830389E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShy_Cg</pointname>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>-0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>107.0</x>
-              <y>258.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RShyCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RShy</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1375</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>124.0</x>
-              <y>289.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-17.0</x>
-            <y>-1.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>32.0</x>
-            <y>66.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>31.0</x>
-              <y>21.0</y>
-            </control_point>
-            <control_point>
-              <x>31.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>3.0</x>
-              <y>87.0</y>
-            </control_point>
-            <control_point>
-              <x>3.0</x>
-              <y>21.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RShy_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.8</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_22_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>RElb</bodyname>
-      <parent>
-        <bodyname>RShy</bodyname>
-        <pointname>Point_RShy</pointname>
-      </parent>
-      <joint>
-        <jointname>RElb</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>161.0</x>
-              <y>318.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.63306933</mass>
-      <com>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>-9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>2.2599075E-4</Ixx>
-        <Iyy>2.7781302E-4</Iyy>
-        <Izz>2.1878992E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="RElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_RElb_Cg</pointname>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>-9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>157.0</x>
-              <y>352.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="RElbCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-17.0</x>
-            <y>6.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>30.0</x>
-            <y>46.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>18.0</x>
-              <y>26.0</y>
-            </control_point>
-            <control_point>
-              <x>18.0</x>
-              <y>63.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>63.0</y>
-            </control_point>
-            <control_point>
-              <x>-4.0</x>
-              <y>26.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>RElb_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>0.15</y>
-                <z>-0.67</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_21_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShp</bodyname>
-      <parent>
-        <bodyname>DWYTorso</bodyname>
-        <pointname>Point_DWYTorso_2</pointname>
-      </parent>
-      <joint>
-        <jointname>LShSag</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>423.0</x>
-              <y>107.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.56782202</mass>
-      <com>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>7.8337456E-4</Ixx>
-        <Iyy>6.3399921E-4</Iyy>
-        <Izz>0.001008054</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LShp_Cg</pointname>
-        <coordinates>
-          <x>0.0099500565</x>
-          <y>0.038682024</y>
-          <z>1.6446924E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>474.0</x>
-              <y>95.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShpCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShp</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.071</y>
-          <z>0.0</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>481.0</x>
-              <y>125.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>35.0</x>
-            <y>9.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>circle</shape>
-          <size>
-            <x>31.0</x>
-            <y>31.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>64.0</x>
-              <y>13.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShp_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.08</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_14_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShr</bodyname>
-      <parent>
-        <bodyname>LShp</bodyname>
-        <pointname>Point_LShp</pointname>
-      </parent>
-      <joint>
-        <jointname>LShLat</jointname>
-        <type>R1</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>531.0</x>
-              <y>127.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.77683752</mass>
-      <com>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>-2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>4.5812904E-4</Ixx>
-        <Ixy>2.4111086E-7</Ixy>
-        <Ixz>9.2521185E-6</Ixz>
-        <Iyy>4.3089204E-4</Iyy>
-        <Iyz>4.721695E-7</Iyz>
-        <Izz>3.4174855E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>Point_LShr_Cg</pointname>
-        <coordinates>
-          <x>-0.0038936743</x>
-          <y>-2.2022151E-6</y>
-          <z>-0.0048616005</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>560.0</x>
-              <y>142.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShrCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShr</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.0425</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>541.0</x>
-              <y>165.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-26.0</x>
-            <y>2.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>57.0</x>
-            <y>39.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>68.0</x>
-              <y>24.0</y>
-            </control_point>
-            <control_point>
-              <x>68.0</x>
-              <y>60.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>60.0</y>
-            </control_point>
-            <control_point>
-              <x>32.0</x>
-              <y>24.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShr_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.85</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_13_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LShy</bodyname>
-      <parent>
-        <bodyname>LShr</bodyname>
-        <pointname>Point_LShr</pointname>
-      </parent>
-      <joint>
-        <jointname>LShYaw</jointname>
-        <type>R3</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>573.0</x>
-              <y>212.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0462304</mass>
-      <com>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>0.0030934574</Ixx>
-        <Ixz>3.0222291E-4</Ixz>
-        <Iyy>0.0029181573</Iyy>
-        <Iyz>5.2663661E-4</Iyz>
-        <Izz>8.0830389E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShy_Cg</pointname>
-        <coordinates>
-          <x>0.0022714213</x>
-          <y>0.0071091517</y>
-          <z>-0.057455221</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>590.0</x>
-              <y>248.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LShyCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LShy</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>-0.1375</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>579.0</x>
-              <y>284.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-15.0</x>
-            <y>3.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>31.0</x>
-            <y>71.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>18.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>18.0</x>
-              <y>67.0</y>
-            </control_point>
-            <control_point>
-              <x>-8.0</x>
-              <y>67.0</y>
-            </control_point>
-            <control_point>
-              <x>-8.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LShy_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.8</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_12_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>LElb</bodyname>
-      <parent>
-        <bodyname>LShy</bodyname>
-        <pointname>Point_LShy</pointname>
-      </parent>
-      <joint>
-        <jointname>LElb</jointname>
-        <type>R2</type>
-        <nature>independent</nature>
-        <graphics>
-          <x2D>
-            <position>
-              <x>604.0</x>
-              <y>318.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>0.63306933</mass>
-      <com>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-      </com>
-      <inertia>
-        <Ixx>2.2599075E-4</Ixx>
-        <Ixy>2.637647E-7</Ixy>
-        <Iyy>2.7781302E-4</Iyy>
-        <Iyz>2.0435288E-6</Iyz>
-        <Izz>2.1878992E-4</Izz>
-      </inertia>
-      <point>
-        <pointname>origin</pointname>
-        <coordinates>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </coordinates>
-        <extforce name="LElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
-      </point>
-      <point>
-        <pointname>Point_LElb_Cg</pointname>
-        <coordinates>
-          <x>-6.5553403E-4</x>
-          <y>9.3464748E-4</y>
-          <z>-9.3101289E-4</z>
-        </coordinates>
-        <graphics>
-          <x2D>
-            <position>
-              <x>623.0</x>
-              <y>345.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-        <sensor name="LElbCg" Pos="true" Vit="true" Om="true" />
-      </point>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-12.0</x>
-            <y>4.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>31.0</x>
-            <y>46.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>19.0</x>
-              <y>4.0</y>
-            </control_point>
-            <control_point>
-              <x>19.0</x>
-              <y>50.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>50.0</y>
-            </control_point>
-            <control_point>
-              <x>-12.0</x>
-              <y>4.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D>
-          <x3D_shapes>
-            <inline>
-              <name>LElb_visu</name>
-              <position>
-                <x>-0.02</x>
-                <y>-0.15</y>
-                <z>-0.67</z>
-              </position>
-              <scale>
-                <x>0.03937</x>
-                <y>0.03937</y>
-                <z>0.03937</z>
-              </scale>
-              <rotation>
-                <x>1.5707963267948966</x>
-                <y>0.0</y>
-                <z>0.0</z>
-              </rotation>
-              <color R="0" G="0" B="0" />
-              <transparency>0.0</transparency>
-              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_11_prt.obj</url>
-            </inline>
-          </x3D_shapes>
-        </x3D>
-      </graphics>
-    </body>
-    <body>
-      <bodyname>CameraRobot</bodyname>
-      <parent>
-        <bodyname>CameraPoint</bodyname>
-        <pointname>Point_Camera</pointname>
-      </parent>
-      <joint>
-        <jointname>FJ_R3_CamRob</jointname>
-        <type>R3</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>344.0</x>
-              <y>1274.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R2_CamRob</jointname>
-        <type>R2</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>345.0</x>
-              <y>1340.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T1_CamRob</jointname>
-        <type>T1</type>
-        <nature>driven</nature>
-        <initialvalue>
-          <q>-1.0</q>
-        </initialvalue>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>420.0</x>
-              <y>1335.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0</mass>
-      <inertia>
-        <Ixx>0.001</Ixx>
-        <Iyy>0.001</Iyy>
-        <Izz>0.001</Izz>
-      </inertia>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-20.0</x>
-            <y>-49.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>103.0</x>
-            <y>69.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>83.0</x>
-              <y>-49.0</y>
-            </control_point>
-            <control_point>
-              <x>83.0</x>
-              <y>20.0</y>
-            </control_point>
-            <control_point>
-              <x>-20.0</x>
-              <y>20.0</y>
-            </control_point>
-            <control_point>
-              <x>-20.0</x>
-              <y>-49.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D />
-      </graphics>
-    </body>
-    <body>
-      <bodyname>CameraFree</bodyname>
-      <parent>
-        <bodyname>base</bodyname>
-        <pointname>origin</pointname>
-      </parent>
-      <joint>
-        <jointname>FJ_T1_CamFree</jointname>
-        <type>T1</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>214.0</x>
-              <y>1272.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T2_CamFree</jointname>
-        <type>T2</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>209.0</x>
-              <y>1362.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_T3_CamFree</jointname>
-        <type>T3</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>202.0</x>
-              <y>1455.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R1_CamFree</jointname>
-        <type>R1</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>306.0</x>
-              <y>1459.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R2_CamFree</jointname>
-        <type>R2</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>406.0</x>
-              <y>1459.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <joint>
-        <jointname>FJ_R3_CamFree</jointname>
-        <type>R3</type>
-        <nature>driven</nature>
-        <symbols>
-          <q />
-        </symbols>
-        <graphics>
-          <x2D>
-            <position>
-              <x>506.0</x>
-              <y>1459.0</y>
-            </position>
-          </x2D>
-          <x3D />
-        </graphics>
-      </joint>
-      <mass>1.0</mass>
-      <inertia>
-        <Ixx>0.001</Ixx>
-        <Iyy>0.001</Iyy>
-        <Izz>0.001</Izz>
-      </inertia>
-      <graphics>
-        <x2D>
-          <position>
-            <x>-50.0</x>
-            <y>-50.0</y>
-          </position>
-          <name_pos>
-            <x>0.0</x>
-            <y>0.0</y>
-          </name_pos>
-          <shape>rectangle</shape>
-          <size>
-            <x>100.0</x>
-            <y>75.0</y>
-          </size>
-          <control_points>
-            <control_point>
-              <x>50.0</x>
-              <y>-50.0</y>
-            </control_point>
-            <control_point>
-              <x>50.0</x>
-              <y>25.0</y>
-            </control_point>
-            <control_point>
-              <x>-50.0</x>
-              <y>25.0</y>
-            </control_point>
-            <control_point>
-              <x>-50.0</x>
-              <y>-50.0</y>
-            </control_point>
-          </control_points>
-          <angle>0.0</angle>
-        </x2D>
-        <x3D />
-      </graphics>
-    </body>
-  </bodytree>
-  <user_models>
-    <!--this is the userModel list-->
-    <user_model>
-      <name>cpp</name>
-      <parameter>
-        <name>interface</name>
-        <type>structure</type>
-        <structure>void</structure>
-        <header />
-      </parameter>
-    </user_model>
-  </user_models>
-  <x3D>
-    <background>
-      <!--This is the background properties-->
-      <color R="255" G="255" B="255" />
-    </background>
-    <shapeScaleFactors>
-      <!--This is the scale factors-->
-      <frame>0.2</frame>
-      <joint>0.3</joint>
-      <com>0.3</com>
-      <point>0.3</point>
-    </shapeScaleFactors>
-    <viewpoints>
-      <!--This is the viewpoint list-->
-      <viewpoint>
-        <name>Camera</name>
-        <joint>FJ_T1_CamRob</joint>
-        <position>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </position>
-        <target>
-          <x>1.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </target>
-        <up>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>1.0</z>
-        </up>
-        <projection>perspective</projection>
-        <scale>0.22</scale>
-        <fieldOfView>1.65</fieldOfView>
-        <frontClipDst>0.01</frontClipDst>
-        <backClipDst>50.0</backClipDst>
-      </viewpoint>
-      <viewpoint>
-        <name>Robot</name>
-        <joint>FJ_T3_Coman</joint>
-        <position>
-          <x>1.0</x>
-          <y>1.0</y>
-          <z>0.5</z>
-        </position>
-        <target>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.0</z>
-        </target>
-        <up>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>1.0</z>
-        </up>
-        <projection>perspective</projection>
-        <scale>0.2</scale>
-        <fieldOfView>1.2</fieldOfView>
-        <frontClipDst>0.01</frontClipDst>
-        <backClipDst>50.0</backClipDst>
-      </viewpoint>
-      <viewpoint>
-        <name>Profile</name>
-        <joint>FJ_T2_Coman</joint>
-        <position>
-          <x>0.0</x>
-          <y>-2.0</y>
-          <z>0.5</z>
-        </position>
-        <target>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>0.5</z>
-        </target>
-        <up>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>1.0</z>
-        </up>
-        <projection>parallel</projection>
-        <scale>0.3</scale>
-        <fieldOfView>0.785</fieldOfView>
-        <frontClipDst>0.01</frontClipDst>
-        <backClipDst>50.0</backClipDst>
-      </viewpoint>
-    </viewpoints>
-    <lights>
-      <!--This is the light list-->
-      <pointLight>
-        <name>X_fwd</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>-5.0</x>
-          <y>5.0</y>
-          <z>1.5</z>
-        </position>
-        <attenuation>
-          <cst>1.5</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-      </pointLight>
-      <pointLight>
-        <name>X_bwd</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>5.0</x>
-          <y>-5.0</y>
-          <z>1.5</z>
-        </position>
-        <attenuation>
-          <cst>1.5</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-      </pointLight>
-      <pointLight>
-        <name>Z</name>
-        <joint>FJ_T3_Coman</joint>
-        <active />
-        <position>
-          <x>0.0</x>
-          <y>0.0</y>
-          <z>5.0</z>
-        </position>
-        <attenuation>
-          <cst>2.0</cst>
-          <lin>0.0</lin>
-          <quad>0.0</quad>
-        </attenuation>
-      </pointLight>
-    </lights>
-  </x3D>
-</mbsDescR7>
-
+<?xml version="1.0" encoding="UTF-8"?>
+<mbsDescR7>
+  <mbsname>coman_short_feet</mbsname>
+  <mbsyspad_version>1.9.4</mbsyspad_version>
+  <base>
+    <!--this is the base body-->
+    <gravity>
+      <coordinates>
+        <x>0.0</x>
+        <y>0.0</y>
+        <z>-9.81</z>
+      </coordinates>
+    </gravity>
+    <baseBodyName>BaseBody</baseBodyName>
+    <graphics>
+      <x2D>
+        <position>
+          <x>220.0</x>
+          <y>1180.0</y>
+        </position>
+        <base_body>
+          <position>
+            <x>-28.0</x>
+            <y>-66.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>126.0</x>
+            <y>89.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>98.0</x>
+              <y>-66.0</y>
+            </control_point>
+            <control_point>
+              <x>98.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-28.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-28.0</x>
+              <y>-66.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </base_body>
+      </x2D>
+      <x3D>
+        <x3D_frame />
+        <x3D_shapes>
+          <inline>
+            <name>Chess_ground</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>1.5707963267948966</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <url>PRJPATH/../../animationR/obj/ground/chess_ground.obj</url>
+          </inline>
+        </x3D_shapes>
+      </x3D>
+    </graphics>
+  </base>
+  <bodytree>
+    <!--this is the body tree-->
+    <body>
+      <bodyname>CameraPoint</bodyname>
+      <parent>
+        <bodyname>base</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_T1_Coman</jointname>
+        <type>T1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>264.0</x>
+              <y>1180.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T2_Coman</jointname>
+        <type>T2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T3_Coman</jointname>
+        <type>T3</type>
+        <nature>independent</nature>
+        <initialvalue>
+          <q>0.5268</q>
+        </initialvalue>
+        <graphics>
+          <x2D>
+            <position>
+              <x>352.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0</mass>
+      <inertia>
+        <Ixx>0.001</Ixx>
+        <Iyy>0.001</Iyy>
+        <Izz>0.001</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_Camera</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>352.0</x>
+              <y>1215.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>3.0</x>
+            <y>-12.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>37.0</x>
+              <y>-12.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+    <body>
+      <bodyname>Waist</bodyname>
+      <parent>
+        <bodyname>CameraPoint</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_R1_Coman</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>317.0</x>
+              <y>770.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_Coman</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>627.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R3_Coman</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>299.0</x>
+              <y>472.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.8008695</mass>
+      <com>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0051032982</Ixx>
+        <Ixz>0.0016920543</Ixz>
+        <Iyy>0.006241636</Iyy>
+        <Iyz>1.7225949E-5</Iyz>
+        <Izz>0.0024517762</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Waist_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_Waist_1</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>438.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_2</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>404.0</x>
+              <y>445.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_3</pointname>
+        <coordinates>
+          <x>0.020281504</x>
+          <y>0.0</y>
+          <z>0.11912053</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>395.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_Cg</pointname>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>362.0</x>
+              <y>419.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="WaistCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-83.0</x>
+            <y>-75.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>190.0</x>
+            <y>96.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>107.0</x>
+              <y>-75.0</y>
+            </control_point>
+            <control_point>
+              <x>107.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>-75.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Waist_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_07_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>164.0</x>
+              <y>435.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Iyy>5.458038E-4</Iyy>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>434.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>125.0</x>
+              <y>399.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-31.0</x>
+            <y>1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-9.0</x>
+              <y>-3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.05</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_20_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighUpLeg</bodyname>
+      <parent>
+        <bodyname>RHipmot</bodyname>
+        <pointname>Point_RHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>506.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Iyy>0.0010484958</Iyy>
+        <Iyz>2.5492874E-5</Iyz>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>609.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>110.0</x>
+              <y>563.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-9.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>28.0</x>
+              <y>-1.0</y>
+            </control_point>
+            <control_point>
+              <x>28.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>-1.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_19_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighUpLeg</bodyname>
+        <pointname>Point_RThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>670.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixy>1.7802399E-6</Ixy>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Iyz>4.522711E-5</Iyz>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>752.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>719.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>83.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_18_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighLowLeg</bodyname>
+        <pointname>Point_RThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>816.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixy>1.1227206E-5</Ixy>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Iyz>5.1899176E-4</Iyz>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RLowLeg_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>90.0</x>
+              <y>911.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>104.0</x>
+              <y>868.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-13.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>95.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>24.0</x>
+              <y>3.0</y>
+            </control_point>
+            <control_point>
+              <x>24.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_17_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFootmot</bodyname>
+      <parent>
+        <bodyname>RLowLeg</bodyname>
+        <pointname>Point_RLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>982.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixy>7.6346303E-6</Ixy>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>1026.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>116.0</x>
+              <y>1008.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>0.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>25.0</x>
+            <y>25.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>25.0</x>
+              <y>23.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_16_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFoot</bodyname>
+      <parent>
+        <bodyname>RFootmot</bodyname>
+        <pointname>Point_RFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>84.0</x>
+              <y>1120.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>1108.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>85.0</x>
+              <y>1194.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>-20.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>77.0</x>
+            <y>112.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>41.0</x>
+              <y>-20.0</y>
+            </control_point>
+            <control_point>
+              <x>41.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>-20.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.036</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <box>
+              <name>Bar_right</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.07</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="150" G="150" B="150" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Right_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/short/coman_short_foot.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>467.0</x>
+              <y>443.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Ixy>3.9936567E-6</Ixy>
+        <Iyy>5.458038E-4</Iyy>
+        <Iyz>3.3389575E-6</Iyz>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>534.0</x>
+              <y>444.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>503.0</x>
+              <y>409.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>36.0</x>
+            <y>0.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>63.0</x>
+              <y>-10.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.04</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_06_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighUpLeg</bodyname>
+      <parent>
+        <bodyname>LHipmot</bodyname>
+        <pointname>Point_LHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>520.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Ixy>1.4449904E-6</Ixy>
+        <Iyy>0.0010484958</Iyy>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>623.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>578.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>24.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>12.0</x>
+              <y>7.0</y>
+            </control_point>
+            <control_point>
+              <x>12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>7.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_05_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighUpLeg</bodyname>
+        <pointname>Point_LThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>521.0</x>
+              <y>680.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>523.0</x>
+              <y>759.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>730.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>82.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>13.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LTighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.078</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_04_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighLowLeg</bodyname>
+        <pointname>Point_LThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>516.0</x>
+              <y>824.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LLowLeg_contact_visu" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>517.0</x>
+              <y>916.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>493.0</x>
+              <y>890.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>10.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>88.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>29.0</x>
+              <y>-17.0</y>
+            </control_point>
+            <control_point>
+              <x>29.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>-17.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_03_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFootmot</bodyname>
+      <parent>
+        <bodyname>LLowLeg</bodyname>
+        <pointname>Point_LLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>512.0</x>
+              <y>985.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Iyz>3.6889276E-6</Iyz>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>1030.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>1009.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-4.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>21.0</x>
+            <y>21.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>39.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_02_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFoot</bodyname>
+      <parent>
+        <bodyname>LFootmot</bodyname>
+        <pointname>Point_LFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1148.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>522.0</x>
+              <y>1144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1221.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-37.0</x>
+            <y>-19.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>72.0</x>
+            <y>108.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>35.0</x>
+              <y>-19.0</y>
+            </control_point>
+            <control_point>
+              <x>35.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>-19.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.037</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <box>
+              <name>Bar_left</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.07</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="150" G="150" B="150" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Left_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/feet/short/coman_short_foot.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWL</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_3</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>305.0</x>
+              <y>331.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.75398402</mass>
+      <com>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.4093348E-4</Ixx>
+        <Iyy>3.9581916E-4</Iyy>
+        <Iyz>1.443019E-6</Iyz>
+        <Izz>3.7449753E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWL</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>294.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWL_Cg</pointname>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>313.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWLCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-52.0</x>
+            <y>-44.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>103.0</x>
+            <y>55.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>21.0</x>
+              <y>-51.0</y>
+            </control_point>
+            <control_point>
+              <x>21.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-51.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>DWL1_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_09_prt.obj</url>
+            </inline>
+            <inline>
+              <name>DWL2_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.039</x>
+                <y>0.039</y>
+                <z>0.039</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_08_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWS</bodyname>
+      <parent>
+        <bodyname>DWL</bodyname>
+        <pointname>Point_DWL</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>310.0</x>
+              <y>245.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.54588774</mass>
+      <com>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.7126578E-4</Ixx>
+        <Ixz>1.1440119E-6</Ixz>
+        <Iyy>7.4686706E-4</Iyy>
+        <Izz>6.2196235E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWS</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0475</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>308.0</x>
+              <y>192.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWS_Cg</pointname>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>223.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWSCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-35.0</x>
+            <y>-47.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>65.0</x>
+            <y>47.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>0.0</x>
+              <y>-18.0</y>
+            </control_point>
+            <control_point>
+              <x>0.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-18.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWYTorso</bodyname>
+      <parent>
+        <bodyname>DWS</bodyname>
+        <pointname>Point_DWS</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>307.0</x>
+              <y>144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>6.3617175</mass>
+      <com>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.034089342</Ixx>
+        <Iyy>0.030117357</Iyy>
+        <Iyz>6.2443771E-6</Iyz>
+        <Izz>0.025296582</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Torso_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_Cg</pointname>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>350.0</x>
+              <y>97.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWYCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_1</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>-0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>252.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_2</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>375.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-62.0</x>
+            <y>-51.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>138.0</x>
+            <y>61.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-1.0</x>
+              <y>-8.0</y>
+            </control_point>
+            <control_point>
+              <x>-1.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-8.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Torso_visu</name>
+              <position>
+                <x>-0.015</x>
+                <y>0.0</y>
+                <z>-0.69</z>
+              </position>
+              <scale>
+                <x>0.0393</x>
+                <y>0.0393</y>
+                <z>0.0393</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/torso_blue.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>105.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Ixy>1.9035768E-4</Ixy>
+        <Iyy>6.3399921E-4</Iyy>
+        <Iyz>4.3840774E-6</Iyz>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>162.0</x>
+              <y>99.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>156.0</x>
+              <y>129.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>30.0</x>
+            <y>30.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>3.0</x>
+              <y>12.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_24_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShr</bodyname>
+      <parent>
+        <bodyname>RShp</bodyname>
+        <pointname>Point_RShp</pointname>
+      </parent>
+      <joint>
+        <jointname>RShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>114.0</x>
+              <y>123.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>80.0</x>
+              <y>141.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>170.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>56.0</x>
+            <y>43.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_23_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShy</bodyname>
+      <parent>
+        <bodyname>RShr</bodyname>
+        <pointname>Point_RShr</pointname>
+      </parent>
+      <joint>
+        <jointname>RShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>123.0</x>
+              <y>226.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixy>6.6702837E-5</Ixy>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>107.0</x>
+              <y>258.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>124.0</x>
+              <y>289.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>-1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>32.0</x>
+            <y>66.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>31.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>31.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_22_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RElb</bodyname>
+      <parent>
+        <bodyname>RShy</bodyname>
+        <pointname>Point_RShy</pointname>
+      </parent>
+      <joint>
+        <jointname>RElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>161.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Iyy>2.7781302E-4</Iyy>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>157.0</x>
+              <y>352.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>6.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>30.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>26.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>26.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_21_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>423.0</x>
+              <y>107.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Iyy>6.3399921E-4</Iyy>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>474.0</x>
+              <y>95.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>481.0</x>
+              <y>125.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>35.0</x>
+            <y>9.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>31.0</x>
+            <y>31.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>64.0</x>
+              <y>13.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_14_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShr</bodyname>
+      <parent>
+        <bodyname>LShp</bodyname>
+        <pointname>Point_LShp</pointname>
+      </parent>
+      <joint>
+        <jointname>LShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>531.0</x>
+              <y>127.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixy>2.4111086E-7</Ixy>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Iyz>4.721695E-7</Iyz>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>560.0</x>
+              <y>142.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>541.0</x>
+              <y>165.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-26.0</x>
+            <y>2.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>57.0</x>
+            <y>39.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>68.0</x>
+              <y>24.0</y>
+            </control_point>
+            <control_point>
+              <x>68.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>24.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShr_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_13_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShy</bodyname>
+      <parent>
+        <bodyname>LShr</bodyname>
+        <pointname>Point_LShr</pointname>
+      </parent>
+      <joint>
+        <jointname>LShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>573.0</x>
+              <y>212.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Iyz>5.2663661E-4</Iyz>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>590.0</x>
+              <y>248.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>579.0</x>
+              <y>284.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>71.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShy_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_12_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LElb</bodyname>
+      <parent>
+        <bodyname>LShy</bodyname>
+        <pointname>Point_LShy</pointname>
+      </parent>
+      <joint>
+        <jointname>LElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>604.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Ixy>2.637647E-7</Ixy>
+        <Iyy>2.7781302E-4</Iyy>
+        <Iyz>2.0435288E-6</Iyz>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>623.0</x>
+              <y>345.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>19.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>19.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LElb_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../../animationR/obj/IIT_files/asm0001_vrml_11_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>CameraRobot</bodyname>
+      <parent>
+        <bodyname>CameraPoint</bodyname>
+        <pointname>Point_Camera</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_R3_CamRob</jointname>
+        <type>R3</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>344.0</x>
+              <y>1274.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_CamRob</jointname>
+        <type>R2</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>345.0</x>
+              <y>1340.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T1_CamRob</jointname>
+        <type>T1</type>
+        <nature>driven</nature>
+        <initialvalue>
+          <q>-1.0</q>
+        </initialvalue>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>420.0</x>
+              <y>1335.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0</mass>
+      <inertia>
+        <Ixx>0.001</Ixx>
+        <Iyy>0.001</Iyy>
+        <Izz>0.001</Izz>
+      </inertia>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-20.0</x>
+            <y>-49.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>103.0</x>
+            <y>69.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>83.0</x>
+              <y>-49.0</y>
+            </control_point>
+            <control_point>
+              <x>83.0</x>
+              <y>20.0</y>
+            </control_point>
+            <control_point>
+              <x>-20.0</x>
+              <y>20.0</y>
+            </control_point>
+            <control_point>
+              <x>-20.0</x>
+              <y>-49.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+    <body>
+      <bodyname>CameraFree</bodyname>
+      <parent>
+        <bodyname>base</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_T1_CamFree</jointname>
+        <type>T1</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>214.0</x>
+              <y>1272.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T2_CamFree</jointname>
+        <type>T2</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>209.0</x>
+              <y>1362.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T3_CamFree</jointname>
+        <type>T3</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>202.0</x>
+              <y>1455.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R1_CamFree</jointname>
+        <type>R1</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>1459.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_CamFree</jointname>
+        <type>R2</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>406.0</x>
+              <y>1459.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R3_CamFree</jointname>
+        <type>R3</type>
+        <nature>driven</nature>
+        <symbols>
+          <q />
+        </symbols>
+        <graphics>
+          <x2D>
+            <position>
+              <x>506.0</x>
+              <y>1459.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0</mass>
+      <inertia>
+        <Ixx>0.001</Ixx>
+        <Iyy>0.001</Iyy>
+        <Izz>0.001</Izz>
+      </inertia>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-50.0</x>
+            <y>-50.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>100.0</x>
+            <y>75.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>50.0</x>
+              <y>-50.0</y>
+            </control_point>
+            <control_point>
+              <x>50.0</x>
+              <y>25.0</y>
+            </control_point>
+            <control_point>
+              <x>-50.0</x>
+              <y>25.0</y>
+            </control_point>
+            <control_point>
+              <x>-50.0</x>
+              <y>-50.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+  </bodytree>
+  <user_models>
+    <!--this is the userModel list-->
+    <user_model>
+      <name>cpp</name>
+      <parameter>
+        <name>interface</name>
+        <type>structure</type>
+        <structure>void</structure>
+        <header />
+      </parameter>
+    </user_model>
+  </user_models>
+  <x3D>
+    <background>
+      <!--This is the background properties-->
+      <color R="255" G="255" B="255" />
+    </background>
+    <shapeScaleFactors>
+      <!--This is the scale factors-->
+      <frame>0.2</frame>
+      <joint>0.3</joint>
+      <com>0.3</com>
+      <point>0.3</point>
+    </shapeScaleFactors>
+    <viewpoints>
+      <!--This is the viewpoint list-->
+      <viewpoint>
+        <name>Profile</name>
+        <joint>FJ_T2_Coman</joint>
+        <position>
+          <x>0.0</x>
+          <y>-2.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.5</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>parallel</projection>
+        <scale>0.3</scale>
+        <fieldOfView>0.785</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+      <viewpoint>
+        <name>Robot</name>
+        <joint>FJ_T3_Coman</joint>
+        <position>
+          <x>1.0</x>
+          <y>1.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>perspective</projection>
+        <scale>1.0</scale>
+        <fieldOfView>1.2</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+      <viewpoint>
+        <name>Camera</name>
+        <joint>FJ_T1_CamRob</joint>
+        <position>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </position>
+        <target>
+          <x>1.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>perspective</projection>
+        <scale>1.0</scale>
+        <fieldOfView>1.65</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+    </viewpoints>
+    <lights>
+      <!--This is the light list-->
+      <directionalLight>
+        <name>Light</name>
+        <joint>FJ_T3_Coman</joint>
+        <active />
+        <direction>
+          <x>-0.20000000298023224</x>
+          <y>1.5</y>
+          <z>-1.0</z>
+        </direction>
+      </directionalLight>
+    </lights>
+  </x3D>
+</mbsDescR7>
+
diff --git a/parallel_simu/CMakeLists.txt b/parallel_simu/CMakeLists.txt
new file mode 100644
index 0000000..0e6d61b
--- /dev/null
+++ b/parallel_simu/CMakeLists.txt
@@ -0,0 +1,451 @@
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be
+#   Contact : info@robotran.be
+#
+# Authors: Nicolas Van der Noot, Nicolas Docquier, Timothee Habra, Olivier Lantsoght
+#
+# CMake running the COMAN project in parallel
+#
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                        PROJECT SPECIFIC OPTIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# possibilities: init_feet ; long_feet ; flex_feet ; short_feet
+set( COMAN_VERSION "short_feet" )
+
+# speed reference [m/s]
+if (NOT CMAKE_V_REF)
+	set (CMAKE_V_REF 0.65)
+endif ( )
+
+# height reference [m]
+if(NOT CMAKE_H_REF)
+	set (CMAKE_H_REF 0.12)
+endif ( )
+
+# length reference [m]
+if(NOT CMAKE_L_REF)
+	set (CMAKE_L_REF 0.40)
+endif ( )
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                       PROJECT MAIN CONFIGURATIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# CMake minimum version
+if (UNIX)
+	cmake_minimum_required(VERSION 2.8.7)
+else()
+	cmake_minimum_required(VERSION 3.4)
+endif ()
+
+# project name
+project (parallelSimu)
+
+# project configuration
+set( CMAKE_C_FLAGS_RELEASE   "-O3" )
+set( CMAKE_CXX_FLAGS_RELEASE "-O3" )
+
+# Variable for storing the path to Robotran common files (should be adapted depending on the location of those source)
+if(NOT DEFINED ROBOTRAN_SOURCE_DIR)
+	set(TRIAL_PATHS_MBSYSC
+		/home/nico/Documents/Robotran/Projects/robotran/mbsysc/MBsysC    # Nico ThinkPad Linux
+		/data/nvander/Documents/Robotran/Projects/robotran/mbsysc/MBsysC # Nico BioRob Linux
+		C:/Users/Nico/Documents/Robotran/Projects/mbsysc/MBsysC          # Nico ThinkPad Windows
+		/Users/MarieSophie/Documents/Robotran/Projects/mbsysc/MBsysC     # Mary Mac OS
+		D:/Documents/Github/mbsysc/MBsysC      # phil asus
+		/home/pgreiner/Documents/mbsysc/MBsysC # phil biorob linux
+		${PROJECT_SOURCE_DIR}/../mbsysCopy # copy folder
+	)
+	find_path(ROBOTRAN_SOURCE_DIR mbs_common ${TRIAL_PATHS_MBSYSC} DOC "Path to the Robotran-MBsysC common files")
+endif ( )
+
+# message to display the project name and COMAN version
+message(STATUS "Processing ${PROJECT_NAME}")
+message(STATUS "MBsysC path: ${ROBOTRAN_SOURCE_DIR}")
+message(STATUS "CoMan version: ${COMAN_VERSION}")
+message(STATUS "Speed ref: ${CMAKE_V_REF}")
+message(STATUS "Height ref: ${CMAKE_H_REF}")
+message(STATUS "Length ref: ${CMAKE_L_REF}")
+
+# for Unix: display all the warnings, except the ones related to /* -- */, to unused variables and to unknown warnings
+if (UNIX)
+	set( CMAKE_C_FLAGS "-g -Wall -Wno-unused-but-set-variable -Wno-unused-variable -Wno-comment -Wno-unknown-warning-option" )
+endif ( )
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         Windows libraries
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+if (WIN32)
+
+	# avoid warnings
+	if(COMMAND cmake_policy)
+		cmake_policy(SET CMP0054 NEW)
+	endif()
+
+	# Visual studio version
+	if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
+		if (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 18.0)
+			message("Visual Studio version set to VS2012 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2012)
+		elseif (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 19.0)
+			message("Visual Studio version set to VS2013 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2013)
+		else ( )
+			message("Visual Studio version set to VS2015 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2015)
+		endif ( )
+	else ( )
+		message(FATAL_ERROR "CMake error: only Visual Studio projects can currently be used on Windows !")
+	endif ( )
+
+	## --- WIN32 or WIN64 DETECTION --- ##
+	if( CMAKE_SIZEOF_VOID_P EQUAL 8 )   # 64bit Windows
+		SET(WIN_LIB_DIRECTORY win64_include_lib)
+	else( )  # 32bit Windows
+		SET(WIN_LIB_DIRECTORY win32_include_lib)
+	endif( )
+
+	## ---- WINDOWS DLL FILES ---- ##
+
+	# copy all the dll (except 'jvm.dll') used for Windows
+	# these dll files are copied in the Executable directory (Debug or Release)
+	
+	# common dll
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+
+	# GLPK dll
+	file(COPY ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_win/glpk/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
+	file(COPY ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_win/glpk/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+
+	# GLFW dll
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+endif ( )
+
+# install prefix
+set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/Debug)
+
+# find libraries paths
+set(CMAKE_MODULE_PATH ${ROBOTRAN_SOURCE_DIR}/conf ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_conf)
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         Additional CMakelists.txt
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# additional CMake functions
+set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
+set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
+
+add_subdirectory ( ${CMAKE_AUX}/flags/     ${CMAKE_AUX_BIN}/flags/     )
+add_subdirectory ( ${CMAKE_AUX}/listing/   ${CMAKE_AUX_BIN}/listing/   )
+add_subdirectory ( ${CMAKE_AUX}/libraries/ ${CMAKE_AUX_BIN}/libraries/ )
+add_subdirectory ( ${CMAKE_AUX}/make_opt/  ${CMAKE_AUX_BIN}/make_opt/  )
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                    PROJECT SPECIFIC OPTIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# symbolic path
+set ( SYMBOLIC_PATH symbolicR/${COMAN_VERSION} )
+
+find_package( SeparateLibs )
+
+# link to MBSysC lib already compiled
+if ( SEPARATE_MBS_COMMON_FOUND )
+
+	message("\n   MBsysC libraries found, FLAG_SEPARATE_BUILD set to ON !")
+	set (FLAG_SEPARATE_BUILD ON)
+
+	# link to symbolic files already compiled
+	if ( SEPARATE_SYMBOLIC_FOUND )
+		message("   Symbolic library found, FLAG_SEPARATE_SYMBOLIC set to ON !\n")
+		set (FLAG_SEPARATE_SYMBOLIC ON)
+	else ( )
+		message("   Symbolic library not found, FLAG_SEPARATE_SYMBOLIC set to OFF !\n")
+		set (FLAG_SEPARATE_SYMBOLIC OFF)
+	endif ( )
+
+# compile the whole project
+else ( )
+	message("\n   Not all MBsysC libraries found, compiling the whole project !\n")
+	set (FLAG_SEPARATE_BUILD OFF)
+	set (FLAG_SEPARATE_SYMBOLIC OFF)
+endif ( )
+
+# link to user function files already compiled (always off)
+set (FLAG_SEPARATE_USER_FCT OFF)
+
+# no traditional real-time features
+option (FLAG_REAL_TIME "Real time" OFF)
+option (FLAG_JAVA "Real time (Java 3D visualization)" OFF)
+option (FLAG_OPEN_GL "Real time (OpenGL 3D visualization)" OFF)
+option (FLAG_PLOT "Plot and interact" OFF)
+option (FLAG_VISU "Visualization" OFF)
+
+# CMake functions
+release_debug()
+make_options()
+
+# MBsysC files to compile
+if (NOT FLAG_SEPARATE_BUILD)
+	add_subdirectory( ${ROBOTRAN_SOURCE_DIR}/mbs_common ${CMAKE_CURRENT_BINARY_DIR}/mbs_common )
+	flags_check()
+	definitions()
+endif ( )
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                             EXTRA LIBRARIES
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# GLPK library
+add_definitions( -D GLPK_LIB )
+find_package( GLPK REQUIRED )
+if (GLPK_FOUND)
+	include_directories (${GLPK_INCLUDE_PATH})
+	message( "\n  >> main: GLPK found")
+else ( )
+	message( "\n  >> main: GLPK not found, modify ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_conf/FindGLPK.cmake")
+endif ( )
+
+message("      -> INC : ${GLPK_INCLUDE_PATH}")
+message("      -> LIB : ${GLPK_LIBRARIES}\n")
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         FLAGS RELATED FUNCTIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+
+# debug or release
+if( CMAKE_BUILD_TYPE MATCHES "Debug" )
+	add_definitions( -D DEBUG_VERSION )
+	message(STATUS "Debug version !")
+endif( )
+
+# Windows M_PI definitions
+if (WIN32)
+	add_definitions(-D _USE_MATH_DEFINES)
+endif (WIN32)
+
+# Robotran simulation flag
+add_definitions ( -D ROBOTRAN_SIMU )
+
+# OpenGL special configuration
+add_definitions(-D SEPARATE_OPEN_GL)
+
+# Parallel simulation
+add_definitions(-D PARALLEL_SIMU)
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                           USER ABSOLUTE PATHS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# name of the build folder (relative to main CMakelists.txt)
+set(BUILD_PATH ${PROJECT_BINARY_DIR})
+file(RELATIVE_PATH BUILD_PATH_REL ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR})
+
+# upper-case letters of COMAN version
+string(TOUPPER ${COMAN_VERSION} COMAN_VERSION_UPPER)
+
+# configure a header file to pass some of the CMake settings to the source code
+configure_file (
+	"${ROBOTRAN_SOURCE_DIR}/conf/cmake_config.h.in"
+	"${PROJECT_BINARY_DIR}/conf/cmake_config.h"
+)
+
+configure_file (
+	"${PROJECT_SOURCE_DIR}/../workR/src/conf/cmake_define.h.in"
+	"${PROJECT_BINARY_DIR}/conf/cmake_define.h"
+)
+
+# OpenGL config
+set(OPEN_GL_FILES_PATH ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_realtime/open_gl/)
+
+configure_file (
+    "${OPEN_GL_FILES_PATH}/conf/cmake_open_gl_config.h.in"
+    "${PROJECT_BINARY_DIR}/conf/cmake_open_gl_config.h"
+)
+
+# 'cmake_config.h.in' and 'cmake_open_gl_config.h.in' is in the 'conf' folder
+include_directories (${PROJECT_BINARY_DIR}/conf)
+
+# Libxml2
+xml_lib(main)
+
+# OpenGL library
+open_gl_lib(main)
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                           LIST FILES
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ 
+# list source files to compile
+init_src()
+
+increment_src( ${PROJECT_SOURCE_DIR}/src )
+
+increment_src( ${OPEN_GL_FILES_PATH} )
+
+# list include directories (to find headers)
+init_include()
+
+increment_include( ${PROJECT_SOURCE_DIR}/src )
+increment_include( ${PROJECT_SOURCE_DIR}/../userfctR )
+increment_include( ${PROJECT_SOURCE_DIR}/../userFiles )
+increment_include( ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH} )
+increment_include( ${ROBOTRAN_SOURCE_DIR}/mbs_common )
+
+increment_include( ${OPEN_GL_FILES_PATH} )
+
+# SDL.h header
+if ( FLAG_PLOT )
+	sdl_header_lib(project)
+endif ( )
+
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         EXECUTABLE COMPILATION
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# include these directories
+include_directories (${INCLUDE_DIR})
+
+# name of the executable
+set (Executable exe_${PROJECT_NAME})
+
+# generate the executable
+add_executable ( ${Executable} ${SOURCE_FILES} )
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         EXECUTABLE LINKING
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+flags_clean()
+
+if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
+
+	find_path(LibRobotranC_DIR LibRobotranCConfig.cmake "${ROBOTRAN_SOURCE_DIR}/build")
+	find_package( LibRobotranC REQUIRED )
+	target_link_libraries( ${Executable} ${LIB_MBSYSC_MODULES} ${LIB_MBSYSC_LOAD} ${LIB_MBSYSC_UTILITIES} )
+	add_definitions(${LIB_MBSYSC_DEFINITIONS})
+
+	# user function and symbolic libraries
+	target_link_libraries ( ${Executable} Project_userfct )
+
+	if ( FLAG_SEPARATE_SYMBOLIC )
+		target_link_libraries ( ${Executable} ${LIB_SYMBOLIC} )
+	endif ( )
+
+	if (WIN32)
+
+		## ---- WINDOWS DLL FILES ---- ##
+
+		# find dll directories (Debug or Release)
+		get_filename_component(LIB_MBSYSC_MODULES_DIR ${LIB_MBSYSC_MODULES} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_LOAD_DIR ${LIB_MBSYSC_LOAD} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_UTILITIES_DIR ${LIB_MBSYSC_UTILITIES} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_REALTIME_DIR ${LIB_MBSYSC_REALTIME} DIRECTORY)
+
+		# copy all the dll used for Windows
+		# these dll files are copied in the Executable directory (Debug)
+		file(COPY ${LIB_MBSYSC_MODULES_DIR}\\MBsysC_module.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_LOAD_DIR}\\MBsysC_loadXML.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_UTILITIES_DIR}\\MBsysC_utilities.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_REALTIME_DIR}\\MBsysC_realtime.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+
+	endif ( )
+
+else ( )
+
+	target_link_libraries( ${Executable} MBsysC_loadXML MBsysC_module )
+
+	if (NOT FLAG_SHARED_LIB)
+
+		# find MBSysC static libraries
+		target_link_libraries( ${Executable} MBsysC_numerics MBsysC_realtime MBsysC_utilities )
+
+		#user functions library
+		target_link_libraries ( ${Executable} Project_userfct )
+		target_link_libraries ( ${Executable} Project_symbolic )
+
+		#Libxml2 and GSL external libraries
+		target_link_libraries ( ${Executable} ${LIBXML2_LIBRARIES} ${GSL_LIBRARIES} )
+
+		# SDL external library
+		if (FLAG_PLOT)
+			target_link_libraries ( ${Executable} ${SDL2_LIBRARIES} ${SDL2TTF_LIBRARIES} )
+		endif ( )
+
+		# Java external library
+		if (FLAG_JAVA)
+			target_link_libraries ( ${Executable} ${JNI_LIBRARIES} )
+		endif ( )
+
+		# OpenGL external library
+		if (FLAG_OPEN_GL)
+
+		endif ( )
+
+	endif ( )
+
+endif ( )
+
+# link library: Libxml2
+target_link_libraries ( ${Executable} ${LIBXML2_LIBRARIES} )
+
+# link library: OpenGL
+target_link_libraries ( ${Executable} ${OPEN_GL_LIBS})
+
+# include MBsysC directories
+include_directories(${LIB_MBSYSC_INCLUDE_DIRS})
+
+# symbolic files
+if ( FLAG_SEPARATE_SYMBOLIC )
+	if( UNIX )
+		file(COPY ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH}/build/libProject_symbolic.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
+	else() # Windows (to be tested)
+		file(COPY ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH}/build/Debug/libProject_symbolic.dll DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Debug)
+	endif()
+else ( )
+	add_subdirectory( ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH} ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
+endif ( )
+
+# user fonction files
+if ( FLAG_SEPARATE_USER_FCT )
+	if( UNIX )
+		file(COPY ${PROJECT_SOURCE_DIR}/../userfctR/build/libProject_userfct.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
+	else() # Windows (to be tested)
+		file(COPY ${PROJECT_SOURCE_DIR}/../userfctR/build/Debug/libProject_userfct.dll DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Debug)
+	endif()
+else ( )
+	add_subdirectory( ${PROJECT_SOURCE_DIR}/../userfctR ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
+endif ( )
+
+# dynamic linking library
+if (UNIX)
+	target_link_libraries ( ${Executable} dl )
+endif ( )
+
+# math external library (for Unix)
+if (UNIX)
+	target_link_libraries ( ${Executable} m )
+endif ( )
+
+# GLPK library
+target_link_libraries (${Executable} ${GLPK_LIBRARIES})
+
diff --git a/parallel_simu/config/Readme.txt b/parallel_simu/config/Readme.txt
new file mode 100644
index 0000000..0652fc8
--- /dev/null
+++ b/parallel_simu/config/Readme.txt
@@ -0,0 +1 @@
+This file is used to save this "config" folder with Git.
diff --git a/parallel_simu/dataR/coman_full_visu.mbs b/parallel_simu/dataR/coman_full_visu.mbs
new file mode 100644
index 0000000..f6fe7ef
--- /dev/null
+++ b/parallel_simu/dataR/coman_full_visu.mbs
@@ -0,0 +1,3571 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<mbsDescR7>
+  <mbsname>coman_short_feet</mbsname>
+  <mbsyspad_version>1.9.4</mbsyspad_version>
+  <base>
+    <!--this is the base body-->
+    <gravity>
+      <coordinates>
+        <x>0.0</x>
+        <y>0.0</y>
+        <z>-9.81</z>
+      </coordinates>
+    </gravity>
+    <baseBodyName>BaseBody</baseBodyName>
+    <graphics>
+      <x2D>
+        <position>
+          <x>220.0</x>
+          <y>1180.0</y>
+        </position>
+        <base_body>
+          <position>
+            <x>-28.0</x>
+            <y>-66.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>126.0</x>
+            <y>89.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>98.0</x>
+              <y>-66.0</y>
+            </control_point>
+            <control_point>
+              <x>98.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-28.0</x>
+              <y>23.0</y>
+            </control_point>
+            <control_point>
+              <x>-28.0</x>
+              <y>-66.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </base_body>
+      </x2D>
+      <x3D>
+        <x3D_frame />
+        <x3D_shapes>
+          <inline>
+            <name>Chess_ground</name>
+            <position>
+              <x>0.0</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </position>
+            <scale>
+              <x>1.0</x>
+              <y>1.0</y>
+              <z>1.0</z>
+            </scale>
+            <rotation>
+              <x>1.5707963267948966</x>
+              <y>0.0</y>
+              <z>0.0</z>
+            </rotation>
+            <color R="0" G="0" B="0" />
+            <transparency>0.0</transparency>
+            <url>PRJPATH/../animationR/obj/ground/chess_ground.obj</url>
+          </inline>
+        </x3D_shapes>
+      </x3D>
+    </graphics>
+  </base>
+  <bodytree>
+    <!--this is the body tree-->
+    <body>
+      <bodyname>Waist</bodyname>
+      <parent>
+        <bodyname>base</bodyname>
+        <pointname>origin</pointname>
+      </parent>
+      <joint>
+        <jointname>FJ_T1_Coman</jointname>
+        <type>T1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>264.0</x>
+              <y>1180.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T2_Coman</jointname>
+        <type>T2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_T3_Coman</jointname>
+        <type>T3</type>
+        <nature>independent</nature>
+        <initialvalue>
+          <q>0.5268</q>
+        </initialvalue>
+        <graphics>
+          <x2D>
+            <position>
+              <x>352.0</x>
+              <y>1181.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R1_Coman</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>317.0</x>
+              <y>770.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R2_Coman</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>627.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <joint>
+        <jointname>FJ_R3_Coman</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>299.0</x>
+              <y>472.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.8008695</mass>
+      <com>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0051032982</Ixx>
+        <Ixz>0.0016920543</Ixz>
+        <Iyy>0.006241636</Iyy>
+        <Iyz>1.7225949E-5</Iyz>
+        <Izz>0.0024517762</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Waist_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_Waist_1</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>438.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_2</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.023</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>404.0</x>
+              <y>445.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_3</pointname>
+        <coordinates>
+          <x>0.020281504</x>
+          <y>0.0</y>
+          <z>0.11912053</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>306.0</x>
+              <y>395.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_Waist_Cg</pointname>
+        <coordinates>
+          <x>-0.026391145</x>
+          <y>-5.8216364E-4</y>
+          <z>0.052632312</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>362.0</x>
+              <y>419.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="WaistCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-83.0</x>
+            <y>-75.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>190.0</x>
+            <y>96.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>107.0</x>
+              <y>-75.0</y>
+            </control_point>
+            <control_point>
+              <x>107.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>-83.0</x>
+              <y>-75.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Waist_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_07_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>164.0</x>
+              <y>435.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Iyy>5.458038E-4</Iyy>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>434.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>-0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>125.0</x>
+              <y>399.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-31.0</x>
+            <y>1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-9.0</x>
+              <y>-3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.05</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_20_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighUpLeg</bodyname>
+      <parent>
+        <bodyname>RHipmot</bodyname>
+        <pointname>Point_RHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>506.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Iyy>0.0010484958</Iyy>
+        <Iyz>2.5492874E-5</Iyz>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>97.0</x>
+              <y>609.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>-0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>110.0</x>
+              <y>563.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-9.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>28.0</x>
+              <y>-1.0</y>
+            </control_point>
+            <control_point>
+              <x>28.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>61.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>-1.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_19_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RThighLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighUpLeg</bodyname>
+        <pointname>Point_RThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>670.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixy>1.7802399E-6</Ixy>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Iyz>4.522711E-5</Iyz>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>752.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>719.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>83.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RThighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_18_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RLowLeg</bodyname>
+      <parent>
+        <bodyname>RThighLowLeg</bodyname>
+        <pointname>Point_RThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>816.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixy>1.1227206E-5</Ixy>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Iyz>5.1899176E-4</Iyz>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RLowLeg_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>90.0</x>
+              <y>911.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>-0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>104.0</x>
+              <y>868.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-13.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>95.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>24.0</x>
+              <y>3.0</y>
+            </control_point>
+            <control_point>
+              <x>24.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>91.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>3.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_17_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFootmot</bodyname>
+      <parent>
+        <bodyname>RLowLeg</bodyname>
+        <pointname>Point_RLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>94.0</x>
+              <y>982.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixy>7.6346303E-6</Ixy>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>91.0</x>
+              <y>1026.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_RFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>-9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>116.0</x>
+              <y>1008.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>0.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>25.0</x>
+            <y>25.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>25.0</x>
+              <y>23.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_16_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RFoot</bodyname>
+      <parent>
+        <bodyname>RFootmot</bodyname>
+        <pointname>Point_RFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>RAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>84.0</x>
+              <y>1120.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>109.0</x>
+              <y>1108.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>85.0</x>
+              <y>1194.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="RFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>-20.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>77.0</x>
+            <y>112.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>41.0</x>
+              <y>-20.0</y>
+            </control_point>
+            <control_point>
+              <x>41.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>92.0</y>
+            </control_point>
+            <control_point>
+              <x>-36.0</x>
+              <y>-20.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.036</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <box>
+              <name>Bar_right</name>
+              <position>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.07</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="150" G="150" B="150" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Right_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/short/coman_short_foot.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LHipmot</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>467.0</x>
+              <y>443.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.89258227</mass>
+      <com>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.7672294E-4</Ixx>
+        <Ixy>3.9936567E-6</Ixy>
+        <Iyy>5.458038E-4</Iyy>
+        <Iyz>3.3389575E-6</Iyz>
+        <Izz>5.126859E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LHipmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0496</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>534.0</x>
+              <y>444.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LHipmot_Cg</pointname>
+        <coordinates>
+          <x>-3.269678E-4</x>
+          <y>0.023587321</y>
+          <z>-2.6496509E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>503.0</x>
+              <y>409.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LHipmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>36.0</x>
+            <y>0.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>34.0</x>
+            <y>34.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>63.0</x>
+              <y>-10.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LHip_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.04</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_06_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighUpLeg</bodyname>
+      <parent>
+        <bodyname>LHipmot</bodyname>
+        <pointname>Point_LHipmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>520.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0246101</mass>
+      <com>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.165719E-4</Ixx>
+        <Ixy>1.4449904E-6</Ixy>
+        <Iyy>0.0010484958</Iyy>
+        <Izz>6.8637197E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LThighUpLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1024</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>519.0</x>
+              <y>623.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighUpLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0012591632</x>
+          <y>0.016896372</y>
+          <z>-0.058903336</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>578.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighUpLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>24.0</x>
+            <y>101.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>12.0</x>
+              <y>7.0</y>
+            </control_point>
+            <control_point>
+              <x>12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>108.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>7.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LThighUp_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.52</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_05_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LThighLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighUpLeg</bodyname>
+        <pointname>Point_LThighUpLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LHipYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>521.0</x>
+              <y>680.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.7001134</mass>
+      <com>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0038979546</Ixx>
+        <Ixz>2.6403262E-5</Ixz>
+        <Iyy>0.0039360845</Iyy>
+        <Izz>9.0218711E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LThigh_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1234</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>523.0</x>
+              <y>759.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LThighLowLeg_Cg</pointname>
+        <coordinates>
+          <x>9.8720058E-4</x>
+          <y>-6.7037262E-4</y>
+          <z>-0.075716843</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>730.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LThighLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>25.0</x>
+            <y>82.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>13.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>13.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>86.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LTighLow_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.078</y>
+                <z>-0.42</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_04_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LLowLeg</bodyname>
+      <parent>
+        <bodyname>LThighLowLeg</bodyname>
+        <pointname>Point_LThighLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LKneeSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>516.0</x>
+              <y>824.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.4098179</mass>
+      <com>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0040604004</Ixx>
+        <Ixz>1.2260973E-5</Ixz>
+        <Iyy>0.0040056528</Iyy>
+        <Izz>0.0012414054</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LLowLeg_contact_visu" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LLowLeg</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.201</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>517.0</x>
+              <y>916.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LLowLeg_Cg</pointname>
+        <coordinates>
+          <x>0.0024612666</x>
+          <y>0.0053099614</y>
+          <z>-0.085989484</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>493.0</x>
+              <y>890.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LLowLegCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>10.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>28.0</x>
+            <y>88.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>29.0</x>
+              <y>-17.0</y>
+            </control_point>
+            <control_point>
+              <x>29.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>52.0</y>
+            </control_point>
+            <control_point>
+              <x>4.0</x>
+              <y>-17.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LLowLeg_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.3</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_03_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFootmot</bodyname>
+      <parent>
+        <bodyname>LLowLeg</bodyname>
+        <pointname>Point_LLowLeg</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>512.0</x>
+              <y>985.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.72992131</mass>
+      <com>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.2034425E-4</Ixx>
+        <Ixz>2.5430158E-7</Ixz>
+        <Iyy>3.6584358E-4</Iyy>
+        <Iyz>3.6889276E-6</Iyz>
+        <Izz>3.6065156E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFootmot</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>509.0</x>
+              <y>1030.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_LFootmot_Cg</pointname>
+        <coordinates>
+          <x>8.8067281E-4</x>
+          <y>9.2224881E-4</y>
+          <z>2.3038476E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>488.0</x>
+              <y>1009.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootmotCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-4.0</x>
+            <y>23.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>21.0</x>
+            <y>21.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>39.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LFootMot_visu</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.08</y>
+                <z>-0.1</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_02_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LFoot</bodyname>
+      <parent>
+        <bodyname>LFootmot</bodyname>
+        <pointname>Point_LFootmot</pointname>
+      </parent>
+      <joint>
+        <jointname>LAnkSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1148.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.489756</mass>
+      <com>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.3467E-4</Ixx>
+        <Iyy>8.0402E-4</Iyy>
+        <Izz>0.0011</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LFoot_Cg</pointname>
+        <coordinates>
+          <x>0.01</x>
+          <y>0.0</y>
+          <z>-0.095</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>522.0</x>
+              <y>1144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LFootCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LFoot_F</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>495.0</x>
+              <y>1221.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <extforce name="LFoot_Force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-37.0</x>
+            <y>-19.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>72.0</x>
+            <y>108.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>35.0</x>
+              <y>-19.0</y>
+            </control_point>
+            <control_point>
+              <x>35.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>89.0</y>
+            </control_point>
+            <control_point>
+              <x>-37.0</x>
+              <y>-19.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Cache_left</name>
+              <position>
+                <x>0.0</x>
+                <y>0.041</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <inline>
+              <name>Cache_right</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.037</y>
+                <z>-0.005</z>
+              </position>
+              <scale>
+                <x>0.85</x>
+                <y>0.85</y>
+                <z>0.85</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/cache/cache_foot.obj</url>
+            </inline>
+            <box>
+              <name>Bar_left</name>
+              <position>
+                <x>0.0</x>
+                <y>-0.0</y>
+                <z>-0.06</z>
+              </position>
+              <scale>
+                <x>0.035</x>
+                <y>0.07</y>
+                <z>0.05</z>
+              </scale>
+              <rotation>
+                <x>0.0</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="150" G="150" B="150" />
+              <transparency>0.0</transparency>
+              <size>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </size>
+            </box>
+            <inline>
+              <name>Left_foot</name>
+              <position>
+                <x>-0.06</x>
+                <y>-0.0525</y>
+                <z>-0.0795</z>
+              </position>
+              <scale>
+                <x>1.0</x>
+                <y>1.0</y>
+                <z>1.0</z>
+              </scale>
+              <rotation>
+                <x>-1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/feet/short/coman_short_foot.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWL</bodyname>
+      <parent>
+        <bodyname>Waist</bodyname>
+        <pointname>Point_Waist_3</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>305.0</x>
+              <y>331.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.75398402</mass>
+      <com>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>3.4093348E-4</Ixx>
+        <Iyy>3.9581916E-4</Iyy>
+        <Iyz>1.443019E-6</Iyz>
+        <Izz>3.7449753E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWL</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>304.0</x>
+              <y>294.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWL_Cg</pointname>
+        <coordinates>
+          <x>-0.0021273416</x>
+          <y>0.0033575235</y>
+          <z>-0.0010076896</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>313.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWLCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-52.0</x>
+            <y>-44.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>103.0</x>
+            <y>55.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>21.0</x>
+              <y>-51.0</y>
+            </control_point>
+            <control_point>
+              <x>21.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-3.0</y>
+            </control_point>
+            <control_point>
+              <x>1.0</x>
+              <y>-51.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>DWL1_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_09_prt.obj</url>
+            </inline>
+            <inline>
+              <name>DWL2_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.0</y>
+                <z>-0.64</z>
+              </position>
+              <scale>
+                <x>0.039</x>
+                <y>0.039</y>
+                <z>0.039</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_08_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWS</bodyname>
+      <parent>
+        <bodyname>DWL</bodyname>
+        <pointname>Point_DWL</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>310.0</x>
+              <y>245.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.54588774</mass>
+      <com>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>9.7126578E-4</Ixx>
+        <Ixz>1.1440119E-6</Ixz>
+        <Iyy>7.4686706E-4</Iyy>
+        <Izz>6.2196235E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_DWS</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0475</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>308.0</x>
+              <y>192.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWS_Cg</pointname>
+        <coordinates>
+          <x>4.2999707E-4</x>
+          <y>-0.0097650317</y>
+          <z>0.033011157</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>336.0</x>
+              <y>223.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWSCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-35.0</x>
+            <y>-47.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>65.0</x>
+            <y>47.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>0.0</x>
+              <y>-18.0</y>
+            </control_point>
+            <control_point>
+              <x>0.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-6.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-18.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D />
+      </graphics>
+    </body>
+    <body>
+      <bodyname>DWYTorso</bodyname>
+      <parent>
+        <bodyname>DWS</bodyname>
+        <pointname>Point_DWS</pointname>
+      </parent>
+      <joint>
+        <jointname>WaistYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>307.0</x>
+              <y>144.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>6.3617175</mass>
+      <com>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.034089342</Ixx>
+        <Iyy>0.030117357</Iyy>
+        <Iyz>6.2443771E-6</Iyz>
+        <Izz>0.025296582</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="Torso_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_Cg</pointname>
+        <coordinates>
+          <x>-0.03585012</x>
+          <y>1.8286043E-5</y>
+          <z>0.117557148</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>350.0</x>
+              <y>97.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="DWYCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_1</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>-0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>252.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <point>
+        <pointname>Point_DWYTorso_2</pointname>
+        <coordinates>
+          <x>-0.015</x>
+          <y>0.0825</y>
+          <z>0.1554</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>375.0</x>
+              <y>114.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-62.0</x>
+            <y>-51.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>138.0</x>
+            <y>61.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>-1.0</x>
+              <y>-8.0</y>
+            </control_point>
+            <control_point>
+              <x>-1.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-5.0</y>
+            </control_point>
+            <control_point>
+              <x>-2.0</x>
+              <y>-8.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>Torso_visu</name>
+              <position>
+                <x>-0.035</x>
+                <y>0.0</y>
+                <z>-0.69</z>
+              </position>
+              <scale>
+                <x>0.0393</x>
+                <y>0.0393</y>
+                <z>0.0393</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/torso_blue.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_1</pointname>
+      </parent>
+      <joint>
+        <jointname>RShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>217.0</x>
+              <y>105.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Ixy>1.9035768E-4</Ixy>
+        <Iyy>6.3399921E-4</Iyy>
+        <Iyz>4.3840774E-6</Iyz>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>-0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>162.0</x>
+              <y>99.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>-0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>156.0</x>
+              <y>129.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>7.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>30.0</x>
+            <y>30.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>3.0</x>
+              <y>12.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShp_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_24_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShr</bodyname>
+      <parent>
+        <bodyname>RShp</bodyname>
+        <pointname>Point_RShp</pointname>
+      </parent>
+      <joint>
+        <jointname>RShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>114.0</x>
+              <y>123.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_RShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>80.0</x>
+              <y>141.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>105.0</x>
+              <y>170.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-36.0</x>
+            <y>5.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>56.0</x>
+            <y>43.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>32.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>65.0</y>
+            </control_point>
+            <control_point>
+              <x>-22.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShr_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_23_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RShy</bodyname>
+      <parent>
+        <bodyname>RShr</bodyname>
+        <pointname>Point_RShr</pointname>
+      </parent>
+      <joint>
+        <jointname>RShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>123.0</x>
+              <y>226.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixy>6.6702837E-5</Ixy>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>-0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>107.0</x>
+              <y>258.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>124.0</x>
+              <y>289.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>-1.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>32.0</x>
+            <y>66.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>31.0</x>
+              <y>21.0</y>
+            </control_point>
+            <control_point>
+              <x>31.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>87.0</y>
+            </control_point>
+            <control_point>
+              <x>3.0</x>
+              <y>21.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RShy_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_22_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>RElb</bodyname>
+      <parent>
+        <bodyname>RShy</bodyname>
+        <pointname>Point_RShy</pointname>
+      </parent>
+      <joint>
+        <jointname>RElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>161.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Iyy>2.7781302E-4</Iyy>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="RElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_RElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>-9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>157.0</x>
+              <y>352.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="RElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-17.0</x>
+            <y>6.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>30.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>26.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>63.0</y>
+            </control_point>
+            <control_point>
+              <x>-4.0</x>
+              <y>26.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>RElb_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_21_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShp</bodyname>
+      <parent>
+        <bodyname>DWYTorso</bodyname>
+        <pointname>Point_DWYTorso_2</pointname>
+      </parent>
+      <joint>
+        <jointname>LShSag</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>423.0</x>
+              <y>107.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.56782202</mass>
+      <com>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>7.8337456E-4</Ixx>
+        <Iyy>6.3399921E-4</Iyy>
+        <Izz>0.001008054</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShp_Cg</pointname>
+        <coordinates>
+          <x>0.0099500565</x>
+          <y>0.038682024</y>
+          <z>1.6446924E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>474.0</x>
+              <y>95.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShpCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShp</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.071</y>
+          <z>0.0</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>481.0</x>
+              <y>125.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>35.0</x>
+            <y>9.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>circle</shape>
+          <size>
+            <x>31.0</x>
+            <y>31.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>64.0</x>
+              <y>13.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShp_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.08</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_14_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShr</bodyname>
+      <parent>
+        <bodyname>LShp</bodyname>
+        <pointname>Point_LShp</pointname>
+      </parent>
+      <joint>
+        <jointname>LShLat</jointname>
+        <type>R1</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>531.0</x>
+              <y>127.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.77683752</mass>
+      <com>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>4.5812904E-4</Ixx>
+        <Ixy>2.4111086E-7</Ixy>
+        <Ixz>9.2521185E-6</Ixz>
+        <Iyy>4.3089204E-4</Iyy>
+        <Iyz>4.721695E-7</Iyz>
+        <Izz>3.4174855E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>Point_LShr_Cg</pointname>
+        <coordinates>
+          <x>-0.0038936743</x>
+          <y>-2.2022151E-6</y>
+          <z>-0.0048616005</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>560.0</x>
+              <y>142.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShrCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShr</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.0425</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>541.0</x>
+              <y>165.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-26.0</x>
+            <y>2.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>57.0</x>
+            <y>39.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>68.0</x>
+              <y>24.0</y>
+            </control_point>
+            <control_point>
+              <x>68.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>60.0</y>
+            </control_point>
+            <control_point>
+              <x>32.0</x>
+              <y>24.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShr_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.15</y>
+                <z>-0.85</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_13_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LShy</bodyname>
+      <parent>
+        <bodyname>LShr</bodyname>
+        <pointname>Point_LShr</pointname>
+      </parent>
+      <joint>
+        <jointname>LShYaw</jointname>
+        <type>R3</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>573.0</x>
+              <y>212.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>1.0462304</mass>
+      <com>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>0.0030934574</Ixx>
+        <Ixz>3.0222291E-4</Ixz>
+        <Iyy>0.0029181573</Iyy>
+        <Iyz>5.2663661E-4</Iyz>
+        <Izz>8.0830389E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LShoulder_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy_Cg</pointname>
+        <coordinates>
+          <x>0.0022714213</x>
+          <y>0.0071091517</y>
+          <z>-0.057455221</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>590.0</x>
+              <y>248.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LShyCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LShy</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>-0.1375</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>579.0</x>
+              <y>284.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-15.0</x>
+            <y>3.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>71.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>18.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>18.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>67.0</y>
+            </control_point>
+            <control_point>
+              <x>-8.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LShy_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.15</y>
+                <z>-0.8</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_12_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+    <body>
+      <bodyname>LElb</bodyname>
+      <parent>
+        <bodyname>LShy</bodyname>
+        <pointname>Point_LShy</pointname>
+      </parent>
+      <joint>
+        <jointname>LElb</jointname>
+        <type>R2</type>
+        <nature>independent</nature>
+        <graphics>
+          <x2D>
+            <position>
+              <x>604.0</x>
+              <y>318.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+      </joint>
+      <mass>0.63306933</mass>
+      <com>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+      </com>
+      <inertia>
+        <Ixx>2.2599075E-4</Ixx>
+        <Ixy>2.637647E-7</Ixy>
+        <Iyy>2.7781302E-4</Iyy>
+        <Iyz>2.0435288E-6</Iyz>
+        <Izz>2.1878992E-4</Izz>
+      </inertia>
+      <point>
+        <pointname>origin</pointname>
+        <coordinates>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </coordinates>
+        <extforce name="LElbow_contact_force" Pos="true" Rot="true" Vit="true" Om="true" />
+      </point>
+      <point>
+        <pointname>Point_LElb_Cg</pointname>
+        <coordinates>
+          <x>-6.5553403E-4</x>
+          <y>9.3464748E-4</y>
+          <z>-9.3101289E-4</z>
+        </coordinates>
+        <graphics>
+          <x2D>
+            <position>
+              <x>623.0</x>
+              <y>345.0</y>
+            </position>
+          </x2D>
+          <x3D />
+        </graphics>
+        <sensor name="LElbCg" Pos="true" Vit="true" Om="true" />
+      </point>
+      <graphics>
+        <x2D>
+          <position>
+            <x>-12.0</x>
+            <y>4.0</y>
+          </position>
+          <name_pos>
+            <x>0.0</x>
+            <y>0.0</y>
+          </name_pos>
+          <shape>rectangle</shape>
+          <size>
+            <x>31.0</x>
+            <y>46.0</y>
+          </size>
+          <control_points>
+            <control_point>
+              <x>19.0</x>
+              <y>4.0</y>
+            </control_point>
+            <control_point>
+              <x>19.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>50.0</y>
+            </control_point>
+            <control_point>
+              <x>-12.0</x>
+              <y>4.0</y>
+            </control_point>
+          </control_points>
+          <angle>0.0</angle>
+        </x2D>
+        <x3D>
+          <x3D_shapes>
+            <inline>
+              <name>LElb_visu</name>
+              <position>
+                <x>-0.02</x>
+                <y>-0.15</y>
+                <z>-0.67</z>
+              </position>
+              <scale>
+                <x>0.03937</x>
+                <y>0.03937</y>
+                <z>0.03937</z>
+              </scale>
+              <rotation>
+                <x>1.5707963267948966</x>
+                <y>0.0</y>
+                <z>0.0</z>
+              </rotation>
+              <color R="0" G="0" B="0" />
+              <transparency>0.0</transparency>
+              <url>PRJPATH/../animationR/obj/IIT_files/asm0001_vrml_11_prt.obj</url>
+            </inline>
+          </x3D_shapes>
+        </x3D>
+      </graphics>
+    </body>
+  </bodytree>
+  <user_models>
+    <!--this is the userModel list-->
+    <user_model>
+      <name>cpp</name>
+      <parameter>
+        <name>interface</name>
+        <type>structure</type>
+        <structure>void</structure>
+        <header />
+      </parameter>
+    </user_model>
+  </user_models>
+  <x3D>
+    <background>
+      <!--This is the background properties-->
+      <color R="255" G="255" B="255" />
+    </background>
+    <shapeScaleFactors>
+      <!--This is the scale factors-->
+      <frame>0.2</frame>
+      <joint>0.3</joint>
+      <com>0.3</com>
+      <point>0.3</point>
+    </shapeScaleFactors>
+    <viewpoints>
+      <!--This is the viewpoint list-->
+      <viewpoint>
+        <name>Profile</name>
+        <joint>FJ_T2_Coman</joint>
+        <position>
+          <x>0.0</x>
+          <y>-2.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.5</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>parallel</projection>
+        <scale>0.2</scale>
+        <fieldOfView>0.785</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+      <viewpoint>
+        <name>Robot</name>
+        <joint>FJ_T3_Coman</joint>
+        <position>
+          <x>1.0</x>
+          <y>1.0</y>
+          <z>0.5</z>
+        </position>
+        <target>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>0.0</z>
+        </target>
+        <up>
+          <x>0.0</x>
+          <y>0.0</y>
+          <z>1.0</z>
+        </up>
+        <projection>perspective</projection>
+        <scale>1.0</scale>
+        <fieldOfView>1.2</fieldOfView>
+        <frontClipDst>0.01</frontClipDst>
+        <backClipDst>50.0</backClipDst>
+      </viewpoint>
+    </viewpoints>
+    <lights>
+      <!--This is the light list-->
+      <directionalLight>
+        <name>Light</name>
+        <joint>FJ_T3_Coman</joint>
+        <active />
+        <direction>
+          <x>-0.20000000298023224</x>
+          <y>1.5</y>
+          <z>-1.0</z>
+        </direction>
+      </directionalLight>
+    </lights>
+  </x3D>
+</mbsDescR7>
+
diff --git a/parallel_simu/src/main.cc b/parallel_simu/src/main.cc
new file mode 100644
index 0000000..3a533d7
--- /dev/null
+++ b/parallel_simu/src/main.cc
@@ -0,0 +1,631 @@
+/*! 
+ * \author Nicolas Van der Noot
+ * \file main.cc
+ * \brief main function for the parallel computation
+ */
+
+#include "MbsWorld3D.hh"
+#include "cmake_config.h"
+#include "AnimReader.hh"
+
+#include "mbs_data.h"
+#include "mbs_part.h"
+
+#include "user_IO.h"
+#include "user_model.h"
+#include "mbs_load_xml.h"
+#include "cmake_config.h"
+#include "random_ctrl.hh"
+#include "coman_properties.hh"
+#include "user_simu_functions.h"
+#include "SimuOptions.h"
+#include "user_all_id.h"
+#include "mbs_set.h"
+#include "RunInOut.hh"
+#include "CppInterface.hh"
+#include "NicoCtrl.hh"
+#include "ComputeManager.hh"
+#include "WalkStates.hh"
+#include "SwingStanceState.hh"
+#include "TrailingState.hh"
+#include "SensorsInfo.hh"
+#include "MotorCoManIndex.hh"
+#include "WholeFeet.hh"
+#include "ForwardKinematics.hh"
+#include "WholeBody.hh"
+#include "BodyPart.hh"
+#include "StimWalkCtrl.hh"
+#include "Matsuoka2D.hh"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+	#include "mbs_dirdyn.h"
+	#include "mbs_project_interface.h"
+	#include "integrator.h"
+#ifdef __cplusplus
+}
+#endif
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+
+#include <unistd.h>
+
+// static structures
+static Inputs_ctrl *ivs;
+static MotorCoManIndex *coman_index;
+
+#define NB_JOINTS_VISU 29 ///< number of joints for the 3D visualization
+
+#define NB_FLOATING 3 ///< number of joints of the floating base
+
+// states from the controller
+#define NB_TIME 1     ///< number of time values
+#define NB_SW_ST 3    ///< number of swing-stance states
+#define NB_MUSCLES 52 ///< number of muscle lengths
+#define NB_NEURONS 10 ///< number of neurons
+
+#define NB_MAIN_STATES (2*NB_FLOATING + 4*NB_MOTORS + NB_TIME + NB_SW_ST + NB_MUSCLES + 2*NB_NEURONS)
+
+#define ID_CTRL_START (2*NB_FLOATING + 4*NB_MOTORS)
+
+void simu_constraint_apply(MbsData *mbs_data);
+bool check_existing_file(const std::string &name);
+double string_double(std::string str);
+bool is_float_string(std::string test_string);
+void apply_state(std::string config_txt, MbsData *mbs_data, MbsDirdyn *mbs_dirdyn);
+
+void controller_inputs(MbsData *mbs_data, MotorCoManIndex *coman_index, Inputs_ctrl *ivs, SensorsInfo *sens_info);
+
+void mbs_simp();
+
+/*! \brief main function
+ */
+int main(int argc, char **argv)
+{
+	// variables declaration
+	double last_t_visu;
+	double *joint_vec;
+	OpenGLMbs::AnimReader *reader_list;
+
+	MbsData *mbs_data;
+	MbsPart *mbs_part;
+	MbsDirdyn *mbs_dirdyn;
+
+	std::string config_txt, start_txt;
+
+	struct timeval timeValue;
+
+	mbs_simp();
+	return 0;
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                   SEED FOR RANDOM                         *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	gettimeofday(&timeValue, NULL);
+	srand(timeValue.tv_usec * timeValue.tv_sec);
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                     LOADING                               *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	// load the .mbs file
+	mbs_data = mbs_load(PROJECT_SOURCE_DIR"/dataR/coman_full_visu.mbs", BUILD_PATH);
+
+	// safety
+	if (mbs_data->Nux != 0)
+	{
+		std::cout << "main (parallel simu) error: no ux should be defined in the .mbs !" << std::endl;
+		exit(EXIT_FAILURE);
+	}
+
+	// initialize the user models for all the motors
+	init_ux_user(mbs_data, 2*NB_MOTORS);
+
+	// apply constraints on the simulation
+	simu_constraint_apply(mbs_data);
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*              COORDINATE PARTITIONING                      *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	mbs_part = mbs_new_part(mbs_data);
+	mbs_part->options->rowperm = 1;
+	mbs_part->options->verbose = 0;
+	mbs_run_part(mbs_part, mbs_data);
+	
+	mbs_delete_part(mbs_part);
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*             DIRECT DYNANMICS : INITIALIZATION             *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	// create new dirdyn module
+	mbs_dirdyn = mbs_new_dirdyn(mbs_data);
+
+	// dirdyn options (see documentations for additional options)
+	mbs_dirdyn->options->dt0 = 250.0e-6;
+
+	// final simulation time
+	mbs_dirdyn->options->tf = 60.0;
+
+	// no file saving
+	mbs_dirdyn->options->save2file = 0;
+
+	// no classic real-time modules
+	mbs_dirdyn->options->realtime = 0;
+
+	// by default, use accelred
+	mbs_dirdyn->options->accelred = 1;
+
+	// inputs - outputs
+	mbs_data->user_IO->run_in_out = new RunInOut();
+
+	// initialize the simulation
+	mbs_dirdyn_init(mbs_dirdyn, mbs_data);
+
+	// static structures
+	coman_index = new MotorCoManIndex(0);
+	ivs = init_Inputs_ctrl(coman_index->NbMot());
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                  OPEN GL INITIALIZATION                   *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	
+	// main window for 3D world
+	OpenGLMbs::MbsWorld3D world_3d(512, 512);
+
+	// load mbs files
+	world_3d.AddMbs(PROJECT_SOURCE_DIR"/dataR/coman_full_visu.mbs");
+
+	// joints for visualization update
+	joint_vec = new double[NB_JOINTS_VISU];
+
+	// last 3D view update time
+	last_t_visu = world_3d.GetT();
+
+	// path of the config file
+	config_txt  = PROJECT_SOURCE_DIR;
+	config_txt += "/config/config.txt";
+
+	// path of the start file
+	start_txt  = PROJECT_SOURCE_DIR;
+	start_txt += "/config/start.txt";
+
+	// wait for the start configuration file
+	while (!check_existing_file(start_txt))
+	{
+		sleep(0.1);
+	}
+
+	// delete config file
+	if( remove( start_txt.c_str() ) != 0 )
+	{
+		std::cout << "main (parallel simu): error deleting " << start_txt << " file !" << std::endl;
+		exit(EXIT_FAILURE);
+	}
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                  DIRECT DYNANMICS : LOOP                  *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	while(mbs_dirdyn->tsim < mbs_dirdyn->options->tf)
+	{
+		// -- dynamics computation -- //
+
+		// user loop
+		user_dirdyn_loop(mbs_data, mbs_dirdyn);
+
+		// function for dirdyn (accelred)
+		mbs_fct_dirdyn(mbs_dirdyn->tsim, mbs_dirdyn->y, mbs_dirdyn->yd, mbs_data, mbs_dirdyn);
+
+		// Runge-Kutta 4
+		rk4(mbs_dirdyn->y, mbs_dirdyn->yd, mbs_dirdyn->nState,
+			mbs_dirdyn->tsim, mbs_dirdyn->dt, mbs_dirdyn->yout,
+			mbs_fct_dirdyn, mbs_data, mbs_dirdyn);
+
+		mbs_dirdyn->tsim += mbs_dirdyn->dt;
+
+		for (int i=0 ; i<mbs_dirdyn->nState ; i++)
+		{
+			mbs_dirdyn->y[i] = mbs_dirdyn->yout[i];
+		}
+
+		// slow down simulation
+		if (mbs_data->tsim > 10.0)
+		{
+			usleep(3000);
+		}
+
+
+		// -- 3D visualization -- //
+ 
+	
+		// update real time
+		world_3d.UpdateTime();
+
+		// window updated each 40 ms (25 Hz)
+		if (world_3d.GetT() - last_t_visu >= 0.04)
+		{
+			for(int i=0; i<NB_JOINTS_VISU; i++)
+			{
+				joint_vec[i] = mbs_data->q[1+i];
+			}
+
+			// update joint positions
+			world_3d.UpdateJoints(0, NB_JOINTS_VISU, joint_vec);
+
+			// update the 3D view
+			world_3d.Update();
+
+			// last 3D view update time
+			last_t_visu = world_3d.GetT();
+		}
+
+		// check if the ESC key was pressed or the world_3d was closed
+		if (!world_3d.CheckClose())
+		{
+			break;
+		}
+
+
+		// -- read external file -- //
+
+		if (check_existing_file(config_txt))
+		{
+			apply_state(config_txt, mbs_data, mbs_dirdyn);
+		}
+	}
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                 DIRECT DYNANMICS : FINISH                 *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	// finish the simulation
+	mbs_dirdyn_finish(mbs_dirdyn, mbs_data);
+
+	// delete dirdyn module
+	mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
+
+	// static structures
+	delete coman_index;
+
+	free_Inputs_ctrl(ivs);
+
+
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+	/*                   CLOSING OPERATIONS                      *
+	/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+	std::cout << "fitness: " << static_cast<RunInOut*>(mbs_data->user_IO->run_in_out)->Fitness() << std::endl;
+
+	// releasing run_in_out
+	delete static_cast<RunInOut*>(mbs_data->user_IO->run_in_out);
+
+	// delete mbs_data module
+	mbs_delete_data(mbs_data);
+
+	delete[] joint_vec;
+
+	return 0;
+}
+
+/*! \brief apply the simulation constraints (if any)
+ *
+ * \param[in,out] mbs_data Robotran structure
+ */
+void simu_constraint_apply(MbsData *mbs_data)
+{
+	SimuOptions *options;
+
+	options = init_SimuOptions();
+
+	switch (options->simu_constraint)
+	{
+		case SIMU_FREE_CONSTRAINT:
+			break;
+
+		case SIMU_2D_CONSTRAINT:
+			mbs_set_qdriven(mbs_data, FJ_T2_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_R1_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_R3_Coman_id);
+			break;
+
+		case SIMU_SKYWALKER:
+			mbs_set_qdriven(mbs_data, FJ_T1_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_T2_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_T3_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_R1_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_R2_Coman_id);
+			mbs_set_qdriven(mbs_data, FJ_R3_Coman_id);
+
+			mbs_data->q[FJ_T3_Coman_id] = 0.75;
+			break;
+
+		default:
+			break;
+	}
+
+	free(options);
+}
+
+/*! \brief check if a file exists
+ *
+ * \param[in] name name (with path) of the file to check
+ * \return true if file exists, 0 otherwise
+ */
+bool check_existing_file(const std::string &name)
+{
+	if (FILE *file = fopen(name.c_str(), "r"))
+	{
+		fclose(file);
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+/*! \brief convert string to double
+ *
+ * \return requested double
+ */
+double string_double(std::string str)
+{
+	double double_val;
+	std::stringstream str_val;
+
+	str_val << str;
+	str_val >> double_val;
+
+	return double_val;
+}
+
+/*! \brief check if a string can be converted to a float
+ *
+ * \return string to test
+ */
+bool is_float_string(std::string test_string)
+{
+	float f;
+	std::istringstream iss(test_string);
+	
+	iss >> std::noskipws >> f;
+
+	return iss.eof() && !iss.fail(); 
+}
+
+/*! \brief apply the new state
+ * 
+ * \param[in] config_txt name (with path) of the configuration file
+ * \param[in] mbs_data main Robotran structure
+ * \param[in] mbs_dirdyn direct dynamics module
+ */
+void apply_state(std::string config_txt, MbsData *mbs_data, MbsDirdyn *mbs_dirdyn)
+{
+	int count;
+	double F_tot[3], T_tot[3];
+
+	SimuCtrl *simu_ctrl;
+	NicoCtrl *ctrl;
+	ComputeManager *manager;
+	WalkStates* walk_states;
+	SwingStanceState *sw_st;
+	TrailingState *tr_st;
+	SensorsInfo *sens_info;
+	WholeFeet *whole_feet;
+	ForwardKinematics *fwd_kin;
+	WholeBody *whole_body;
+	BodyPart *cur_part;
+	StimWalkCtrl *stim_ctrl;
+	Matsuoka2D *matsu;
+
+	std::string cur_line;
+	std::ifstream config_str;
+	std::vector<double> state_vec;
+
+	// open config file
+	config_str.open(config_txt.c_str());
+
+	// fill state vec with the new state values
+	while(!config_str.eof())
+	{
+		std::getline(config_str, cur_line);
+		if (is_float_string(cur_line))
+		{
+			state_vec.push_back(string_double(cur_line));
+		}
+	}
+
+	/*
+	 * nState: 98 -> 2 * 26 + 2 * 23
+	 *
+	 * dydt: [ qd (26) ; qdd (26) ; uxd (23) ; uxdd (23) ]
+	 *    y: [ q  (26) ; qd  (26) ; ux  (23) ; uxd  (23) ]
+	 */
+
+	// safety
+	if (state_vec.size() != NB_MAIN_STATES)
+	{
+		std::cout << "main (parallel simu) error: state_vec size (" << state_vec.size()
+			<< ") != " << NB_MAIN_STATES << std::endl;
+		exit(EXIT_FAILURE);
+	}
+
+	// floating base position
+	for(int i=0; i<NB_FLOATING; i++)
+	{
+		mbs_dirdyn->y[i] = state_vec[i];
+	}
+
+	// floating base velocity
+	for(int i=0; i<NB_FLOATING; i++)
+	{
+		mbs_dirdyn->y[NB_FLOATING+NB_MOTORS+i] = state_vec[NB_FLOATING+i];
+	}
+
+	// joint positions (q)
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		mbs_dirdyn->y[NB_FLOATING+i] = state_vec[2*NB_FLOATING+i];
+	}
+
+	// joint derivatives (qd)
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		mbs_dirdyn->y[2*NB_FLOATING+NB_MOTORS+i] = state_vec[2*NB_FLOATING+NB_MOTORS+i];
+	}
+
+	// motor states and derivatives (ux & uxd)
+	for(int i=0; i<2*NB_MOTORS; i++)
+	{
+		mbs_dirdyn->y[2*NB_FLOATING+2*NB_MOTORS+i] = state_vec[2*NB_FLOATING+2*NB_MOTORS+i];
+
+		mbs_data->ux[i+1] = state_vec[2*NB_FLOATING+2*NB_MOTORS+i];
+	}
+
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		mbs_data->uxd[i+1] = mbs_data->ux[i+1+NB_MOTORS];
+	}
+
+
+	// Update state variables
+	for(int i=1;i<=mbs_data->nqu;i++)
+	{
+		mbs_data->q[mbs_data->qu[i]] = mbs_dirdyn->y[i-1];
+		mbs_data->qd[mbs_data->qu[i]] = mbs_dirdyn->y[i+mbs_data->nqu-1];
+	}
+	for(int i=1;i<=mbs_data->Nux;i++)
+	{
+		mbs_data->ux[i] = mbs_dirdyn->y[i+2*mbs_data->nqu-1];
+	}
+
+	// correct time
+	mbs_data->tsim = state_vec[ID_CTRL_START];
+	mbs_dirdyn->tsim = mbs_data->tsim;
+
+	// simu and controller
+	simu_ctrl = static_cast<CppInterface*>(mbs_data->user_model->cpp.interface)->GetSimuCtrl();
+	ctrl = static_cast<NicoCtrl*>(simu_ctrl->GetCtrl());
+
+	// reset last controller call time
+	simu_ctrl->SetLastTCtrl(0.0);
+
+	manager = ctrl->Manager();
+
+	// sensors info
+	sens_info = static_cast<SensorsInfo*>(simu_ctrl->Computation(SENSORS_INFO));
+	sens_info->Compute();
+
+	// reset ground contact model
+	simu_ctrl->GcmMesh()->ResetGCM();
+	simu_ctrl->GcmMesh()->StateCompute();
+
+	whole_feet = simu_ctrl->GcmMesh()->GetWholeFeet();
+
+	for(int i=0; i<whole_feet->NbBody(); i++)
+	{
+		simu_ctrl->GcmMesh()->Ground()->ComputeFT(F_tot, T_tot, whole_feet->FootBody(i));
+	}
+
+	// actuators integral term
+	simu_ctrl->Actuators()->ResetInt();
+
+	// controller inputs
+	controller_inputs(mbs_data, coman_index, ivs, sens_info);
+
+	ctrl->SetInputs(ivs);
+
+	// forward kinematics
+	fwd_kin = static_cast<ForwardKinematics*>(manager->GetComputation(KINEMATICS_COMPUTE));
+	fwd_kin->Compute();
+
+	// reset ctrl
+	manager->ResetCtrl();
+
+	walk_states = static_cast<WalkStates*>(manager->GetComputation(WALK_STATES));
+
+	// swing legs
+	sw_st = static_cast<SwingStanceState*>(walk_states->State(SWING_STANCE_STATE));
+	sw_st->SetSwingLeg(0, state_vec[ID_CTRL_START+NB_TIME+0]);
+	sw_st->SetSwingLeg(1, state_vec[ID_CTRL_START+NB_TIME+1]);
+
+	// supporting leg
+	sw_st->SetSupportingRLeg(state_vec[ID_CTRL_START+NB_TIME+2]);
+
+	sw_st->ResetCtrlState();
+
+	tr_st = static_cast<TrailingState*>(walk_states->State(TRAILING_STATE));
+	tr_st->ResetCtrlState();
+
+	// muscle lengths
+	whole_body = static_cast<WholeBody*>(manager->GetComputation(WHOLE_BODY));
+
+	count = 0;
+
+	for(int i=0; i<NB_BODY_PARTS; i++)
+	{
+		cur_part = whole_body->GetPart(i);
+
+		for(int j=0; j<cur_part->NbMuscles(); j++)
+		{
+			cur_part->GetMuscle(j)->SetLce(state_vec[ID_CTRL_START+NB_TIME+NB_SW_ST+count]);
+			cur_part->GetMuscle(j)->UpdateLce();
+			count++;
+		}
+	}
+
+	// muscles safety
+	if (count != NB_MUSCLES)
+	{
+		std::cout << "main error: uncorrect number of muscles: " << count << " >< " << NB_MUSCLES << " !" << std::endl;
+	}
+
+	// cpg
+	stim_ctrl = static_cast<StimWalkCtrl*>(whole_body->StimCtrl());
+	matsu = static_cast<Matsuoka2D*>(stim_ctrl->Osc());
+
+	count = 0;
+
+	for(int i=0; i<matsu->NbNeurons(); i++)
+	{
+		matsu->SetX(i, state_vec[ID_CTRL_START+NB_TIME+NB_SW_ST+NB_MUSCLES+count]);
+		count++;
+	}
+
+	for(int i=0; i<matsu->NbNeurons(); i++)
+	{
+		matsu->SetV(i, state_vec[ID_CTRL_START+NB_TIME+NB_SW_ST+NB_MUSCLES+count]);
+		count++;
+	}
+
+	// muscles safety
+	if (count != 2*NB_NEURONS)
+	{
+		std::cout << "main error: uncorrect number of neurons states (x & v): " << count << " >< " << 2*NB_NEURONS << " !" << std::endl;
+	}
+	
+	// close config file
+	config_str.close();
+
+	// delete config file
+	if( remove( config_txt.c_str() ) != 0 )
+	{
+		std::cout << "main (parallel simu): error deleting " << config_txt << " file !" << std::endl;
+		exit(EXIT_FAILURE);
+	}
+}
diff --git a/parallel_simu/src/mbs_simp.cc b/parallel_simu/src/mbs_simp.cc
new file mode 100644
index 0000000..481987e
--- /dev/null
+++ b/parallel_simu/src/mbs_simp.cc
@@ -0,0 +1,437 @@
+/*! 
+ * \author Nicolas Van der Noot
+ * \file mbs_simp.cc
+ * \brief compute a simplified mbs model
+ */
+
+#include "cmake_config.h"
+#include "mbs_part.h"
+#include "mbs_load_xml.h"
+#include "user_all_id.h"
+
+extern "C" {
+	#include "mbs_sensor.h"
+	#include "mbs_project_interface.h"
+}
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+#define DEG_TO_RAD (M_PI / 180.0)
+
+/*! \brief compute the inertia matrix in another position, using steiner
+ * 
+ * \param[out] I_P inertia matrix moved from G to P
+ * \param[in] I_G inertia matrix at the center of gravtity (G)
+ * \param[in] d vector from P to G
+ * \param[in] m mass [kg]
+ */
+void steiner_matrix(double I_P[3][3], double I_G[3][3], double d[3], double m)
+{
+	double d_0_sq, d_1_sq, d_2_sq;
+
+	d_0_sq = d[0] * d[0];
+	d_1_sq = d[1] * d[1];
+	d_2_sq = d[2] * d[2];
+
+	I_P[0][0] = I_G[0][0] + m * (d_1_sq + d_2_sq);
+	I_P[0][1] = I_G[0][1] - m * d[0] * d[1];
+	I_P[0][2] = I_G[0][2] - m * d[0] * d[2];
+
+	I_P[1][0] = I_G[1][0] - m * d[0] * d[1];
+	I_P[1][1] = I_G[1][1] + m * (d_0_sq + d_2_sq);
+	I_P[1][2] = I_G[1][2] - m * d[1] * d[2];
+
+	I_P[2][0] = I_G[2][0] - m * d[0] * d[2];
+	I_P[2][1] = I_G[2][1] - m * d[1] * d[2];
+	I_P[2][2] = I_G[2][2] + m * (d_0_sq + d_1_sq);
+}
+
+/*! \brief multiply a matrix by a vector (i.e. b = M * a)
+ * 
+ * \param[in] M input matrix
+ * \param[in] a input vector
+ * \param[out] b output vector
+ */
+double mat_vec_mult(double M[3][3], double a[3], double b[3])
+{
+	b[0] = M[0][0] * a[0] + M[0][1] * a[1] + M[0][2] * a[2];
+	b[1] = M[1][0] * a[0] + M[1][1] * a[1] + M[1][2] * a[2];
+	b[2] = M[2][0] * a[0] + M[2][1] * a[1] + M[2][2] * a[2];
+}
+
+/*! \brief multiply a matrix by a vector (i.e. b = M * a), with indexes starting at 1
+ * 
+ * \param[in] M input matrix
+ * \param[in] a input vector
+ * \param[out] b output vector
+ */
+double mat_vec_mult_id1(double M[4][4], double a[4], double b[4])
+{
+	b[1] = M[1][1] * a[1] + M[1][2] * a[2] + M[1][3] * a[3];
+	b[2] = M[2][1] * a[1] + M[2][2] * a[2] + M[2][3] * a[3];
+	b[3] = M[3][1] * a[1] + M[3][2] * a[2] + M[3][3] * a[3];
+}
+
+/*! \brief multiply a matrix by a matrix (i.e. C = A * B)
+ * 
+ * \param[in] A input matrix
+ * \param[in] B input matrix
+ * \param[out] C output matrix
+ */
+double mat_mat_mult(double A[3][3], double B[3][3], double C[3][3])
+{
+	C[0][0] = A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0];
+	C[0][1] = A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1];
+	C[0][2] = A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2];
+
+	C[1][0] = A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0];
+	C[1][1] = A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1];
+	C[1][2] = A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2];
+
+	C[2][0] = A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0];
+	C[2][1] = A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1];
+	C[2][2] = A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2];
+}
+
+
+/*! \brief multiply a matrix by a matrix (i.e. C = A * B), with indexes starting at 1
+ * 
+ * \param[in] A input matrix
+ * \param[in] B input matrix
+ * \param[out] C output matrix
+ */
+double mat_mat_mult_id1(double A[4][4], double B[4][4], double C[4][4])
+{
+	C[1][1] = A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1];
+	C[1][2] = A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2];
+	C[1][3] = A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3];
+
+	C[2][1] = A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1];
+	C[2][2] = A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2];
+	C[2][3] = A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3];
+
+	C[3][1] = A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1];
+	C[3][2] = A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2];
+	C[3][3] = A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3];
+}
+
+/*! \brief transpose a matrix
+ * 
+ * \param[in] M input matrix
+ * \param[out] trans_M output transposed matrix
+ */
+void trans_matrix(double M[3][3], double trans_M[3][3])
+{
+	for(int i=0; i<3; i++)
+	{
+		for(int j=0; j<3; j++)
+		{
+			trans_M[j][i] = M[i][j];
+		}
+	}
+}
+
+/*! \brief transpose a matrix, with indexes starting at 1
+ * 
+ * \param[in] M input matrix
+ * \param[out] trans_M output transposed matrix
+ */
+void trans_matrix_id1(double M[4][4], double trans_M[4][4])
+{
+	for(int i=1; i<=3; i++)
+	{
+		for(int j=1; j<=3; j++)
+		{
+			trans_M[j][i] = M[i][j];
+		}
+	}
+}
+
+/*! \brief print the results for mass, com, inertia
+ *
+ * \param[in] new_mass new mass
+ * \param[in] new_com new com
+ * \param[in] new_I_G new inertia (related to 'new_com')
+ * \param[in] body_name name of the new body
+ */
+void print_m_com_I(double new_mass, double new_com[3], double new_I_G[3][3], std::string body_name)
+{
+	std::cout << "====== " << body_name << " ======" << std::endl;
+
+	std::cout << "--- mass ---"  << std::endl;
+	std::cout << new_mass << std::endl;
+	std::cout << std::endl;
+
+	std::cout << "--- COM ---"  << std::endl;
+	for(int i=0; i<3; i++)
+	{
+		std::cout << new_com[i] << std::endl;
+	}
+	std::cout << std::endl;
+
+	std::cout << "--- Inertia ---"  << std::endl;
+	for(int i=0; i<3; i++)
+	{
+		for(int j=0; j<3; j++)
+		{
+			std::cout << new_I_G[i][j] << " ";
+		}
+		std::cout << std::endl;
+	}
+	std::cout << std::endl;
+}
+
+/*! \brief compute the new mass, com and inertia matrix
+ * 
+ * \param[in] ids ids of the bodies parent joints
+ * \param[in] mbs_data Robotran main structure
+ * \param[in] sensor Robotran sensor
+ * \param[out] new_mass new mass
+ * \param[out] new_com new com
+ * \param[out] new_I_G new inertia (related to 'new_com')
+ */
+void mass_com_inertia(std::vector<int> &ids, MbsData *mbs_data, MbsSensor &sensor,
+	double &new_mass, double new_com[3], double new_I_G[3][3])
+{
+	int nb_joints;
+
+	double cur_mass;
+	double new_com_abs[3], ref_pos[3], cur_pos[3], cur_d[3];
+	double l_com[4], l_com_R[4];
+	double R_T_id1[4][4];
+	double R_T[3][3], cur_I_P[3][3], cur_I_G[3][3], cur_I_G_R[3][3];
+
+	nb_joints = mbs_data->njoint;
+
+	// mass
+	new_mass = 0.0;
+	for(unsigned int i=0; i<ids.size(); i++)
+	{
+		new_mass += mbs_data->m[ids[i]];
+	}
+
+	// COM
+	for(int i=0; i<3; i++)
+	{
+		init_sensor(&sensor, nb_joints);
+		mbs_gensensor(&sensor, mbs_data, ids[0]);
+
+		new_com[i] = 0.0;
+		ref_pos[i] = sensor.P[i+1];
+	}
+
+	for(int i=0; i<ids.size(); i++)
+	{
+		init_sensor(&sensor, nb_joints);
+		mbs_gensensor(&sensor, mbs_data, ids[i]);
+
+		for(int j=1; j<=3; j++)
+		{
+			l_com[j] = mbs_data->l[j][ids[i]];
+		}
+
+		trans_matrix_id1(sensor.R, R_T_id1);
+		mat_vec_mult_id1(R_T_id1, l_com, l_com_R);
+
+		cur_mass = mbs_data->m[ids[i]];
+
+		for(int j=0; j<3; j++)
+		{
+			cur_pos[j] = sensor.P[j+1];
+			new_com[j] += cur_mass * (cur_pos[j] + l_com_R[j+1] - ref_pos[j]);
+		}
+	}
+	for(int i=0; i<3; i++)
+	{
+		new_com[i] /= new_mass;
+		new_com_abs[i] = ref_pos[i] + new_com[i];
+	}
+
+
+	// inertia matrix
+	for(int i=0; i<3; i++)
+	{
+		for(int j=0; j<3; j++)
+		{
+			new_I_G[i][j] = 0.0;
+		}
+	}
+
+	for(int i=0; i<ids.size(); i++)
+	{
+		init_sensor(&sensor, nb_joints);
+		mbs_gensensor(&sensor, mbs_data, ids[i]);
+
+		for(int j=1; j<=3; j++)
+		{
+			l_com[j] = mbs_data->l[j][ids[i]];
+		}
+
+		trans_matrix_id1(sensor.R, R_T_id1);
+		mat_vec_mult_id1(R_T_id1, l_com, l_com_R);
+
+		cur_mass = mbs_data->m[ids[i]];
+
+		cur_I_G[0][0] = mbs_data->In[1][ids[i]];
+		cur_I_G[0][1] = mbs_data->In[2][ids[i]];
+		cur_I_G[0][2] = mbs_data->In[3][ids[i]];
+		cur_I_G[1][0] = mbs_data->In[4][ids[i]];
+		cur_I_G[1][1] = mbs_data->In[5][ids[i]];
+		cur_I_G[1][2] = mbs_data->In[6][ids[i]];
+		cur_I_G[2][0] = mbs_data->In[7][ids[i]];
+		cur_I_G[2][1] = mbs_data->In[8][ids[i]];
+		cur_I_G[2][2] = mbs_data->In[9][ids[i]];
+
+		for(int j=0; j<3; j++)
+		{
+			for(int k=0; k<3; k++)
+			{
+				R_T[j][k] = R_T_id1[j+1][k+1];
+			}
+		}
+
+		mat_mat_mult(R_T, cur_I_G, cur_I_G_R);
+
+		for(int j=0; j<3; j++)
+		{
+			cur_pos[j] = sensor.P[j+1];
+			cur_d[j] = cur_pos[j] + l_com_R[j+1] - new_com_abs[j];
+		}
+
+		steiner_matrix(cur_I_P, cur_I_G_R, cur_d, cur_mass);
+
+		for(int j=0; j<3; j++)
+		{
+			for(int k=0; k<3; k++)
+			{
+				new_I_G[j][k] += cur_I_P[j][k];
+			}
+		}
+	}
+}
+
+/*! \brief simplification of a .mbs model
+ */
+void mbs_simp()
+{
+	int nb_joints;
+
+	MbsData *mbs_data;
+	MbsSensor sensor;
+
+	double new_mass;
+	double new_com[3];
+	double new_I_G[3][3];
+
+	std::vector<int> ids;
+
+	// load the .mbs file
+	mbs_data = mbs_load(PROJECT_SOURCE_DIR"/dataR/coman_full_visu.mbs", BUILD_PATH);
+
+	nb_joints = mbs_data->njoint;
+
+	allocate_sensor(&sensor, nb_joints);
+
+	mbs_data->q[RElb_id] = -25.0*DEG_TO_RAD;
+	mbs_data->q[LElb_id] = -25.0*DEG_TO_RAD;
+
+	// Waist + DWL
+	ids.clear();
+	ids.push_back(FJ_R3_Coman_id); // Waist
+	ids.push_back(WaistLat_id);    // DWL
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "Waist + DWL");
+
+
+	// RHipmot + RThighUpLeg + RThighLowLeg
+	ids.clear();
+	ids.push_back(RHipSag_id);
+	ids.push_back(RHipLat_id);
+	ids.push_back(RHipYaw_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "RHipmot + RThighUpLeg + RThighLowLeg");
+
+
+	// RLowLeg + RFootmot
+	ids.clear();
+	ids.push_back(RKneeSag_id);
+	ids.push_back(RAnkLat_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "RLowLeg + RFootmot");
+
+
+	// RFoot
+	ids.clear();
+	ids.push_back(RAnkSag_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "RFoot");
+
+
+	// LHipmot + LThighUpLeg + LThighLowLeg
+	ids.clear();
+	ids.push_back(LHipSag_id);
+	ids.push_back(LHipLat_id);
+	ids.push_back(LHipYaw_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "LHipmot + LThighUpLeg + LThighLowLeg");
+
+
+	// LLowLeg + LFootmot
+	ids.clear();
+	ids.push_back(LKneeSag_id);
+	ids.push_back(LAnkLat_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "LLowLeg + LFootmot");
+
+
+	// LFoot
+	ids.clear();
+	ids.push_back(LAnkSag_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "LFoot");
+
+
+	// DWS + DWYTorso
+	ids.clear();
+	ids.push_back(WaistSag_id);
+	ids.push_back(WaistYaw_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "DWS + DWYTorso");
+
+
+	// RShp + RShr + RShy + RElb
+	ids.clear();
+	ids.push_back(RShSag_id);
+	ids.push_back(RShLat_id);
+	ids.push_back(RShYaw_id);
+	ids.push_back(RElb_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "RShp + RShr + RShy + RElb");
+
+
+	// LShp + LShr + LShy + LElb
+	ids.clear();
+	ids.push_back(LShSag_id);
+	ids.push_back(LShLat_id);
+	ids.push_back(LShYaw_id);
+	ids.push_back(LElb_id);
+
+	mass_com_inertia(ids, mbs_data, sensor,	new_mass, new_com, new_I_G);
+	print_m_com_I(new_mass, new_com, new_I_G, "LShp + LShr + LShy + LElb");
+
+
+	// release memory
+	free_sensor(&sensor);
+	mbs_delete_data(mbs_data);
+}
diff --git a/symbolicR/flex_feet/CMakeLists.txt b/symbolicR/flex_feet/CMakeLists.txt
index 3bae37b..c963c74 100644
--- a/symbolicR/flex_feet/CMakeLists.txt
+++ b/symbolicR/flex_feet/CMakeLists.txt
@@ -1,12 +1,22 @@
-# Copyright: (C)
-# Authors: Timothee Habra & Nicolas Van der Noot
-# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# CMake for compiling the symbolic files of a robotran project in C
+# 
 
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 #                       LIBRARY MAIN CONFIGURATIONS
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
-cmake_minimum_required(VERSION 2.8.7)
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
 
 project(Project_symbolic)
 
@@ -49,10 +59,26 @@ include_directories ( ${INCLUDE_DIR} )
 include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
 
 if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
+
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
     add_library(Project_symbolic SHARED ${SOURCE_FILES})
+	
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_symbolic PROPERTIES SUFFIX .so)    
+    endif()    
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
+        add_custom_command(TARGET Project_symbolic POST_BUILD
+			COMMAND ${CMAKE_COMMAND} -E copy
+                        ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_symbolic.dll
+                        ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_symbolic.dll
+        )
+    endif()
+
 else()
     add_library(Project_symbolic STATIC ${SOURCE_FILES})
-    add_definitions( -DPROJECT_USERFCT_STATIC_DEFINE)
 
     # some symbolic routines call user functions (ext_forces, accelred, ...)
     target_link_libraries(Project_symbolic Project_userfct)
@@ -67,14 +93,5 @@ endif()
 if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
     find_package( LibRobotranC REQUIRED )
     add_definitions(${LIB_MBSYSC_DEFINITIONS})
+    target_link_libraries(Project_symbolic ${LIB_MBSYSC_MODULES})
 endif()
-
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         STUFF FOR WINDOWS (TO BE CLEANED)
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-include (GenerateExportHeader)
-GENERATE_EXPORT_HEADER( Project_symbolic    )
-
-install (TARGETS Project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
diff --git a/symbolicR/flex_feet/user_kinematics_model.c b/symbolicR/flex_feet/user_kinematics_model.c
deleted file mode 100644
index c4a3b14..0000000
--- a/symbolicR/flex_feet/user_kinematics_model.c
+++ /dev/null
@@ -1,2083 +0,0 @@
-#include "mbs_data.h"
-
-#include "math.h"
-
-/*! \brief compute the kinematics model of sensors with ExtForces
- * 
- * \param[in] mbs_data Robotran structure
- * \param[out] PxF_tab tabular with all the 'PxF' computed
- * \param[out] RxF_tab tabular with all the 'RxF' computed
- * \param[out] VxF_tab tabular with all the 'VxF' computed
- * \param[out] OMxF_tab tabular with all the 'OMxF' computed
- */
-void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
-{
-    // variables declaration
-    int i, j;
-    double *q, *qd;
-    double **dpt;
-
-    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4], PxF13[4], PxF14[4];
-    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4], RxF13[4][4], RxF14[4][4];
-    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4], VxF13[4], VxF14[4];
-    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4], OMxF13[4], OMxF14[4];
-
-    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C15, C16, C17, C18, C19, C20, C23, C24, C25, C26, C27, C28, C29, C30, C31, C32, C33;
-    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S15, S16, S17, S18, S19, S20, S23, S24, S25, S26, S27, S28, S29, S30, S31, S32, S33;
-
-    double Dz133, Dz143, Dz213, Dz223;
-    double ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36;
-    double ROcp28_25, ROcp28_35, ROcp28_85, ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87;
-    double ROcp28_97, ROcp28_48, ROcp28_58, ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, OMcp28_25, OMcp28_35;
-    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
-    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
-    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
-    double VIcp28_19, VIcp28_29, VIcp28_39, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_17, ROcp29_27;
-    double ROcp29_37, ROcp29_77, ROcp29_87, ROcp29_97, ROcp29_48, ROcp29_58, ROcp29_68, ROcp29_78, ROcp29_88, ROcp29_98, ROcp29_19, ROcp29_29, ROcp29_39, ROcp29_49, ROcp29_59;
-    double ROcp29_69, ROcp29_110, ROcp29_210, ROcp29_310, ROcp29_710, ROcp29_810, ROcp29_910, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37;
-    double POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37, ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38;
-    double POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38, ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39;
-    double POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39, ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, RLcp29_110, RLcp29_210, RLcp29_310;
-    double POcp29_110, POcp29_210, POcp29_310, OMcp29_110, OMcp29_210, OMcp29_310, ORcp29_110, ORcp29_210, ORcp29_310, VIcp29_110, VIcp29_210, VIcp29_310, ROcp30_25, ROcp30_35, ROcp30_85;
-    double ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56, ROcp30_66, ROcp30_17, ROcp30_27, ROcp30_37, ROcp30_77, ROcp30_87, ROcp30_97, ROcp30_48, ROcp30_58;
-    double ROcp30_68, ROcp30_78, ROcp30_88, ROcp30_98, ROcp30_19, ROcp30_29, ROcp30_39, ROcp30_49, ROcp30_59, ROcp30_69, ROcp30_110, ROcp30_210, ROcp30_310, ROcp30_710, ROcp30_810;
-    double ROcp30_910, ROcp30_411, ROcp30_511, ROcp30_611, ROcp30_711, ROcp30_811, ROcp30_911, ROcp30_112, ROcp30_212, ROcp30_312, ROcp30_712, ROcp30_812, ROcp30_912, OMcp30_25, OMcp30_35;
-    double OMcp30_16, OMcp30_26, OMcp30_36, RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37;
-    double VIcp30_17, VIcp30_27, VIcp30_37, RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38;
-    double VIcp30_18, VIcp30_28, VIcp30_38, RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39;
-    double VIcp30_19, VIcp30_29, VIcp30_39, RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310;
-    double VIcp30_110, VIcp30_210, VIcp30_310, RLcp30_111, RLcp30_211, RLcp30_311, POcp30_111, POcp30_211, POcp30_311, OMcp30_111, OMcp30_211, OMcp30_311, ORcp30_111, ORcp30_211, ORcp30_311;
-    double VIcp30_111, VIcp30_211, VIcp30_311, OMcp30_112, OMcp30_212, OMcp30_312, RLcp30_113, RLcp30_213, RLcp30_313, POcp30_113, POcp30_213, POcp30_313, ORcp30_113, ORcp30_213, ORcp30_313;
-    double VIcp30_113, VIcp30_213, VIcp30_313, ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_17, ROcp31_27;
-    double ROcp31_37, ROcp31_77, ROcp31_87, ROcp31_97, ROcp31_48, ROcp31_58, ROcp31_68, ROcp31_78, ROcp31_88, ROcp31_98, ROcp31_19, ROcp31_29, ROcp31_39, ROcp31_49, ROcp31_59;
-    double ROcp31_69, ROcp31_110, ROcp31_210, ROcp31_310, ROcp31_710, ROcp31_810, ROcp31_910, ROcp31_411, ROcp31_511, ROcp31_611, ROcp31_711, ROcp31_811, ROcp31_911, ROcp31_112, ROcp31_212;
-    double ROcp31_312, ROcp31_712, ROcp31_812, ROcp31_912, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37;
-    double OMcp31_17, OMcp31_27, OMcp31_37, ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38;
-    double OMcp31_18, OMcp31_28, OMcp31_38, ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39;
-    double OMcp31_19, OMcp31_29, OMcp31_39, ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310;
-    double OMcp31_110, OMcp31_210, OMcp31_310, ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311;
-    double OMcp31_111, OMcp31_211, OMcp31_311, ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313;
-    double POcp31_113, POcp31_213, POcp31_313, ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26;
-    double ROcp32_36, ROcp32_46, ROcp32_56, ROcp32_66, ROcp32_115, ROcp32_215, ROcp32_315, ROcp32_715, ROcp32_815, ROcp32_915, ROcp32_416, ROcp32_516, ROcp32_616, ROcp32_716, ROcp32_816;
-    double ROcp32_916, ROcp32_117, ROcp32_217, ROcp32_317, ROcp32_417, ROcp32_517, ROcp32_617, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37;
-    double POcp32_17, POcp32_27, POcp32_37, OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, RLcp32_18, RLcp32_28, RLcp32_38;
-    double POcp32_18, POcp32_28, POcp32_38, OMcp32_18, OMcp32_28, OMcp32_38, ORcp32_18, ORcp32_28, ORcp32_38, VIcp32_18, VIcp32_28, VIcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
-    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
-    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_115, ROcp33_215, ROcp33_315, ROcp33_715, ROcp33_815, ROcp33_915, ROcp33_416, ROcp33_516;
-    double ROcp33_616, ROcp33_716, ROcp33_816, ROcp33_916, ROcp33_117, ROcp33_217, ROcp33_317, ROcp33_417, ROcp33_517, ROcp33_617, ROcp33_118, ROcp33_218, ROcp33_318, ROcp33_718, ROcp33_818;
-    double ROcp33_918, OMcp33_25, OMcp33_35, OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37;
-    double ORcp33_17, ORcp33_27, ORcp33_37, VIcp33_17, VIcp33_27, VIcp33_37, RLcp33_18, RLcp33_28, RLcp33_38, POcp33_18, POcp33_28, POcp33_38, OMcp33_18, OMcp33_28, OMcp33_38;
-    double ORcp33_18, ORcp33_28, ORcp33_38, VIcp33_18, VIcp33_28, VIcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
-    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
-    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
-    double ROcp34_66, ROcp34_115, ROcp34_215, ROcp34_315, ROcp34_715, ROcp34_815, ROcp34_915, ROcp34_416, ROcp34_516, ROcp34_616, ROcp34_716, ROcp34_816, ROcp34_916, ROcp34_117, ROcp34_217;
-    double ROcp34_317, ROcp34_417, ROcp34_517, ROcp34_617, ROcp34_118, ROcp34_218, ROcp34_318, ROcp34_718, ROcp34_818, ROcp34_918, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819;
-    double ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, OMcp34_25, OMcp34_35, OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37;
-    double POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37, VIcp34_17, VIcp34_27, VIcp34_37, RLcp34_18, RLcp34_28, RLcp34_38;
-    double POcp34_18, POcp34_28, POcp34_38, OMcp34_18, OMcp34_28, OMcp34_38, ORcp34_18, ORcp34_28, ORcp34_38, VIcp34_18, VIcp34_28, VIcp34_38, RLcp34_19, RLcp34_29, RLcp34_39;
-    double POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39, ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310;
-    double POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310, ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311;
-    double POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311, ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, OMcp34_112, OMcp34_212, OMcp34_312;
-    double RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85;
-    double ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56, ROcp35_66, ROcp35_115, ROcp35_215, ROcp35_315, ROcp35_715, ROcp35_815, ROcp35_915, ROcp35_416, ROcp35_516;
-    double ROcp35_616, ROcp35_716, ROcp35_816, ROcp35_916, ROcp35_117, ROcp35_217, ROcp35_317, ROcp35_417, ROcp35_517, ROcp35_617, ROcp35_118, ROcp35_218, ROcp35_318, ROcp35_718, ROcp35_818;
-    double ROcp35_918, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, OMcp35_25, OMcp35_35;
-    double OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37, POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37;
-    double VIcp35_17, VIcp35_27, VIcp35_37, RLcp35_18, RLcp35_28, RLcp35_38, POcp35_18, POcp35_28, POcp35_38, OMcp35_18, OMcp35_28, OMcp35_38, ORcp35_18, ORcp35_28, ORcp35_38;
-    double VIcp35_18, VIcp35_28, VIcp35_38, RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39;
-    double VIcp35_19, VIcp35_29, VIcp35_39, RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310;
-    double VIcp35_110, VIcp35_210, VIcp35_310, RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311;
-    double VIcp35_111, VIcp35_211, VIcp35_311, OMcp35_112, OMcp35_212, OMcp35_312, RLcp35_113, RLcp35_213, RLcp35_313, POcp35_113, POcp35_213, POcp35_313, ORcp35_113, ORcp35_213, ORcp35_313;
-    double VIcp35_113, VIcp35_213, VIcp35_313, ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_423, ROcp36_523;
-    double ROcp36_623, ROcp36_723, ROcp36_823, ROcp36_923, ROcp36_124, ROcp36_224, ROcp36_324, ROcp36_724, ROcp36_824, ROcp36_924, ROcp36_125, ROcp36_225, ROcp36_325, ROcp36_425, ROcp36_525;
-    double ROcp36_625, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37, POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37;
-    double ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38, RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39;
-    double OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39, ROcp37_25, ROcp37_35, ROcp37_85, ROcp37_95, ROcp37_16, ROcp37_26;
-    double ROcp37_36, ROcp37_46, ROcp37_56, ROcp37_66, ROcp37_423, ROcp37_523, ROcp37_623, ROcp37_723, ROcp37_823, ROcp37_923, ROcp37_124, ROcp37_224, ROcp37_324, ROcp37_724, ROcp37_824;
-    double ROcp37_924, ROcp37_125, ROcp37_225, ROcp37_325, ROcp37_425, ROcp37_525, ROcp37_625, ROcp37_126, ROcp37_226, ROcp37_326, ROcp37_726, ROcp37_826, ROcp37_926, ROcp37_427, ROcp37_527;
-    double ROcp37_627, ROcp37_727, ROcp37_827, ROcp37_927, ROcp37_128, ROcp37_228, ROcp37_328, ROcp37_428, ROcp37_528, ROcp37_628, OMcp37_25, OMcp37_35, OMcp37_16, OMcp37_26, OMcp37_36;
-    double RLcp37_17, RLcp37_27, RLcp37_37, POcp37_17, POcp37_27, POcp37_37, OMcp37_17, OMcp37_27, OMcp37_37, ORcp37_17, ORcp37_27, ORcp37_37, VIcp37_17, VIcp37_27, VIcp37_37;
-    double OMcp37_18, OMcp37_28, OMcp37_38, RLcp37_19, RLcp37_29, RLcp37_39, POcp37_19, POcp37_29, POcp37_39, OMcp37_19, OMcp37_29, OMcp37_39, ORcp37_19, ORcp37_29, ORcp37_39;
-    double VIcp37_19, VIcp37_29, VIcp37_39, RLcp37_110, RLcp37_210, RLcp37_310, POcp37_110, POcp37_210, POcp37_310, OMcp37_110, OMcp37_210, OMcp37_310, ORcp37_110, ORcp37_210, ORcp37_310;
-    double VIcp37_110, VIcp37_210, VIcp37_310, RLcp37_111, RLcp37_211, RLcp37_311, POcp37_111, POcp37_211, POcp37_311, OMcp37_111, OMcp37_211, OMcp37_311, ORcp37_111, ORcp37_211, ORcp37_311;
-    double VIcp37_111, VIcp37_211, VIcp37_311, RLcp37_112, RLcp37_212, RLcp37_312, POcp37_112, POcp37_212, POcp37_312, OMcp37_112, OMcp37_212, OMcp37_312, ORcp37_112, ORcp37_212, ORcp37_312;
-    double VIcp37_112, VIcp37_212, VIcp37_312, ROcp38_25, ROcp38_35, ROcp38_85, ROcp38_95, ROcp38_16, ROcp38_26, ROcp38_36, ROcp38_46, ROcp38_56, ROcp38_66, ROcp38_423, ROcp38_523;
-    double ROcp38_623, ROcp38_723, ROcp38_823, ROcp38_923, ROcp38_124, ROcp38_224, ROcp38_324, ROcp38_724, ROcp38_824, ROcp38_924, ROcp38_125, ROcp38_225, ROcp38_325, ROcp38_425, ROcp38_525;
-    double ROcp38_625, ROcp38_126, ROcp38_226, ROcp38_326, ROcp38_726, ROcp38_826, ROcp38_926, ROcp38_427, ROcp38_527, ROcp38_627, ROcp38_727, ROcp38_827, ROcp38_927, ROcp38_128, ROcp38_228;
-    double ROcp38_328, ROcp38_428, ROcp38_528, ROcp38_628, ROcp38_129, ROcp38_229, ROcp38_329, ROcp38_729, ROcp38_829, ROcp38_929, OMcp38_25, OMcp38_35, OMcp38_16, OMcp38_26, OMcp38_36;
-    double RLcp38_17, RLcp38_27, RLcp38_37, POcp38_17, POcp38_27, POcp38_37, OMcp38_17, OMcp38_27, OMcp38_37, ORcp38_17, ORcp38_27, ORcp38_37, VIcp38_17, VIcp38_27, VIcp38_37;
-    double OMcp38_18, OMcp38_28, OMcp38_38, RLcp38_19, RLcp38_29, RLcp38_39, POcp38_19, POcp38_29, POcp38_39, OMcp38_19, OMcp38_29, OMcp38_39, ORcp38_19, ORcp38_29, ORcp38_39;
-    double VIcp38_19, VIcp38_29, VIcp38_39, RLcp38_110, RLcp38_210, RLcp38_310, POcp38_110, POcp38_210, POcp38_310, OMcp38_110, OMcp38_210, OMcp38_310, ORcp38_110, ORcp38_210, ORcp38_310;
-    double VIcp38_110, VIcp38_210, VIcp38_310, RLcp38_111, RLcp38_211, RLcp38_311, POcp38_111, POcp38_211, POcp38_311, OMcp38_111, OMcp38_211, OMcp38_311, ORcp38_111, ORcp38_211, ORcp38_311;
-    double VIcp38_111, VIcp38_211, VIcp38_311, RLcp38_112, RLcp38_212, RLcp38_312, POcp38_112, POcp38_212, POcp38_312, OMcp38_112, OMcp38_212, OMcp38_312, ORcp38_112, ORcp38_212, ORcp38_312;
-    double VIcp38_112, VIcp38_212, VIcp38_312, RLcp38_113, RLcp38_213, RLcp38_313, POcp38_113, POcp38_213, POcp38_313, OMcp38_113, OMcp38_213, OMcp38_313, ORcp38_113, ORcp38_213, ORcp38_313;
-    double VIcp38_113, VIcp38_213, VIcp38_313, ROcp39_25, ROcp39_35, ROcp39_85, ROcp39_95, ROcp39_16, ROcp39_26, ROcp39_36, ROcp39_46, ROcp39_56, ROcp39_66, ROcp39_423, ROcp39_523;
-    double ROcp39_623, ROcp39_723, ROcp39_823, ROcp39_923, ROcp39_124, ROcp39_224, ROcp39_324, ROcp39_724, ROcp39_824, ROcp39_924, ROcp39_125, ROcp39_225, ROcp39_325, ROcp39_425, ROcp39_525;
-    double ROcp39_625, ROcp39_130, ROcp39_230, ROcp39_330, ROcp39_730, ROcp39_830, ROcp39_930, ROcp39_431, ROcp39_531, ROcp39_631, ROcp39_731, ROcp39_831, ROcp39_931, ROcp39_132, ROcp39_232;
-    double ROcp39_332, ROcp39_432, ROcp39_532, ROcp39_632, OMcp39_25, OMcp39_35, OMcp39_16, OMcp39_26, OMcp39_36, RLcp39_17, RLcp39_27, RLcp39_37, POcp39_17, POcp39_27, POcp39_37;
-    double OMcp39_17, OMcp39_27, OMcp39_37, ORcp39_17, ORcp39_27, ORcp39_37, VIcp39_17, VIcp39_27, VIcp39_37, OMcp39_18, OMcp39_28, OMcp39_38, RLcp39_19, RLcp39_29, RLcp39_39;
-    double POcp39_19, POcp39_29, POcp39_39, OMcp39_19, OMcp39_29, OMcp39_39, ORcp39_19, ORcp39_29, ORcp39_39, VIcp39_19, VIcp39_29, VIcp39_39, RLcp39_110, RLcp39_210, RLcp39_310;
-    double POcp39_110, POcp39_210, POcp39_310, OMcp39_110, OMcp39_210, OMcp39_310, ORcp39_110, ORcp39_210, ORcp39_310, VIcp39_110, VIcp39_210, VIcp39_310, RLcp39_111, RLcp39_211, RLcp39_311;
-    double POcp39_111, POcp39_211, POcp39_311, OMcp39_111, OMcp39_211, OMcp39_311, ORcp39_111, ORcp39_211, ORcp39_311, VIcp39_111, VIcp39_211, VIcp39_311, RLcp39_112, RLcp39_212, RLcp39_312;
-    double POcp39_112, POcp39_212, POcp39_312, OMcp39_112, OMcp39_212, OMcp39_312, ORcp39_112, ORcp39_212, ORcp39_312, VIcp39_112, VIcp39_212, VIcp39_312, ROcp40_25, ROcp40_35, ROcp40_85;
-    double ROcp40_95, ROcp40_16, ROcp40_26, ROcp40_36, ROcp40_46, ROcp40_56, ROcp40_66, ROcp40_423, ROcp40_523, ROcp40_623, ROcp40_723, ROcp40_823, ROcp40_923, ROcp40_124, ROcp40_224;
-    double ROcp40_324, ROcp40_724, ROcp40_824, ROcp40_924, ROcp40_125, ROcp40_225, ROcp40_325, ROcp40_425, ROcp40_525, ROcp40_625, ROcp40_130, ROcp40_230, ROcp40_330, ROcp40_730, ROcp40_830;
-    double ROcp40_930, ROcp40_431, ROcp40_531, ROcp40_631, ROcp40_731, ROcp40_831, ROcp40_931, ROcp40_132, ROcp40_232, ROcp40_332, ROcp40_432, ROcp40_532, ROcp40_632, ROcp40_133, ROcp40_233;
-    double ROcp40_333, ROcp40_733, ROcp40_833, ROcp40_933, OMcp40_25, OMcp40_35, OMcp40_16, OMcp40_26, OMcp40_36, RLcp40_17, RLcp40_27, RLcp40_37, POcp40_17, POcp40_27, POcp40_37;
-    double OMcp40_17, OMcp40_27, OMcp40_37, ORcp40_17, ORcp40_27, ORcp40_37, VIcp40_17, VIcp40_27, VIcp40_37, OMcp40_18, OMcp40_28, OMcp40_38, RLcp40_19, RLcp40_29, RLcp40_39;
-    double POcp40_19, POcp40_29, POcp40_39, OMcp40_19, OMcp40_29, OMcp40_39, ORcp40_19, ORcp40_29, ORcp40_39, VIcp40_19, VIcp40_29, VIcp40_39, RLcp40_110, RLcp40_210, RLcp40_310;
-    double POcp40_110, POcp40_210, POcp40_310, OMcp40_110, OMcp40_210, OMcp40_310, ORcp40_110, ORcp40_210, ORcp40_310, VIcp40_110, VIcp40_210, VIcp40_310, RLcp40_111, RLcp40_211, RLcp40_311;
-    double POcp40_111, POcp40_211, POcp40_311, OMcp40_111, OMcp40_211, OMcp40_311, ORcp40_111, ORcp40_211, ORcp40_311, VIcp40_111, VIcp40_211, VIcp40_311, RLcp40_112, RLcp40_212, RLcp40_312;
-    double POcp40_112, POcp40_212, POcp40_312, OMcp40_112, OMcp40_212, OMcp40_312, ORcp40_112, ORcp40_212, ORcp40_312, VIcp40_112, VIcp40_212, VIcp40_312, RLcp40_113, RLcp40_213, RLcp40_313;
-    double POcp40_113, POcp40_213, POcp40_313, OMcp40_113, OMcp40_213, OMcp40_313, ORcp40_113, ORcp40_213, ORcp40_313, VIcp40_113, VIcp40_213, VIcp40_313;
-
-    // variables initialization
-    q   = mbs_data->q;
-    qd  = mbs_data->qd;
-    dpt = mbs_data->dpt;
-
-    // cosines and sines
-    C4 = cos(q[4]);
-    C5 = cos(q[5]);
-    C6 = cos(q[6]);
-    C7 = cos(q[7]);
-    C8 = cos(q[8]);
-    C9 = cos(q[9]);
-    C10 = cos(q[10]);
-    C11 = cos(q[11]);
-    C12 = cos(q[12]);
-    C15 = cos(q[15]);
-    C16 = cos(q[16]);
-    C17 = cos(q[17]);
-    C18 = cos(q[18]);
-    C19 = cos(q[19]);
-    C20 = cos(q[20]);
-    C23 = cos(q[23]);
-    C24 = cos(q[24]);
-    C25 = cos(q[25]);
-    C26 = cos(q[26]);
-    C27 = cos(q[27]);
-    C28 = cos(q[28]);
-    C29 = cos(q[29]);
-    C30 = cos(q[30]);
-    C31 = cos(q[31]);
-    C32 = cos(q[32]);
-    C33 = cos(q[33]);
-    S4 = sin(q[4]);
-    S5 = sin(q[5]);
-    S6 = sin(q[6]);
-    S7 = sin(q[7]);
-    S8 = sin(q[8]);
-    S9 = sin(q[9]);
-    S10 = sin(q[10]);
-    S11 = sin(q[11]);
-    S12 = sin(q[12]);
-    S15 = sin(q[15]);
-    S16 = sin(q[16]);
-    S17 = sin(q[17]);
-    S18 = sin(q[18]);
-    S19 = sin(q[19]);
-    S20 = sin(q[20]);
-    S23 = sin(q[23]);
-    S24 = sin(q[24]);
-    S25 = sin(q[25]);
-    S26 = sin(q[26]);
-    S27 = sin(q[27]);
-    S28 = sin(q[28]);
-    S29 = sin(q[29]);
-    S30 = sin(q[30]);
-    S31 = sin(q[31]);
-    S32 = sin(q[32]);
-    S33 = sin(q[33]);
-
-    // main computation
-    Dz133 = q[13]+dpt[3][20];
-    Dz143 = q[14]+dpt[3][21];
-    Dz213 = q[21]+dpt[3][38];
-    Dz223 = q[22]+dpt[3][39];
-    ROcp27_25 = S4*S5;
-    ROcp27_35 = -C4*S5;
-    ROcp27_85 = -S4*C5;
-    ROcp27_95 = C4*C5;
-    ROcp27_16 = C5*C6;
-    ROcp27_26 = ROcp27_25*C6+C4*S6;
-    ROcp27_36 = ROcp27_35*C6+S4*S6;
-    ROcp27_46 = -C5*S6;
-    ROcp27_56 = -ROcp27_25*S6+C4*C6;
-    ROcp27_66 = -ROcp27_35*S6+S4*C6;
-    OMcp27_25 = qd[5]*C4;
-    OMcp27_35 = qd[5]*S4;
-    OMcp27_16 = qd[4]+qd[6]*S5;
-    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
-    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
-    PxF1[1] = q[1];
-    PxF1[2] = q[2];
-    PxF1[3] = q[3];
-    RxF1[1][1] = ROcp27_16;
-    RxF1[1][2] = ROcp27_26;
-    RxF1[1][3] = ROcp27_36;
-    RxF1[2][1] = ROcp27_46;
-    RxF1[2][2] = ROcp27_56;
-    RxF1[2][3] = ROcp27_66;
-    RxF1[3][1] = S5;
-    RxF1[3][2] = ROcp27_85;
-    RxF1[3][3] = ROcp27_95;
-    VxF1[1] = qd[1];
-    VxF1[2] = qd[2];
-    VxF1[3] = qd[3];
-    OMxF1[1] = OMcp27_16;
-    OMxF1[2] = OMcp27_26;
-    OMxF1[3] = OMcp27_36;
-    ROcp28_25 = S4*S5;
-    ROcp28_35 = -C4*S5;
-    ROcp28_85 = -S4*C5;
-    ROcp28_95 = C4*C5;
-    ROcp28_16 = C5*C6;
-    ROcp28_26 = ROcp28_25*C6+C4*S6;
-    ROcp28_36 = ROcp28_35*C6+S4*S6;
-    ROcp28_46 = -C5*S6;
-    ROcp28_56 = -ROcp28_25*S6+C4*C6;
-    ROcp28_66 = -ROcp28_35*S6+S4*C6;
-    ROcp28_17 = ROcp28_16*C7-S5*S7;
-    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
-    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
-    ROcp28_77 = ROcp28_16*S7+S5*C7;
-    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
-    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
-    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
-    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
-    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
-    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
-    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
-    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
-    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
-    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
-    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
-    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
-    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
-    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
-    OMcp28_25 = qd[5]*C4;
-    OMcp28_35 = qd[5]*S4;
-    OMcp28_16 = qd[4]+qd[6]*S5;
-    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
-    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
-    RLcp28_17 = ROcp28_46*dpt[2][2];
-    RLcp28_27 = ROcp28_56*dpt[2][2];
-    RLcp28_37 = ROcp28_66*dpt[2][2];
-    POcp28_17 = q[1]+RLcp28_17;
-    POcp28_27 = q[2]+RLcp28_27;
-    POcp28_37 = q[3]+RLcp28_37;
-    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
-    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
-    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
-    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
-    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
-    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
-    VIcp28_17 = qd[1]+ORcp28_17;
-    VIcp28_27 = qd[2]+ORcp28_27;
-    VIcp28_37 = qd[3]+ORcp28_37;
-    RLcp28_18 = ROcp28_46*dpt[2][6];
-    RLcp28_28 = ROcp28_56*dpt[2][6];
-    RLcp28_38 = ROcp28_66*dpt[2][6];
-    POcp28_18 = POcp28_17+RLcp28_18;
-    POcp28_28 = POcp28_27+RLcp28_28;
-    POcp28_38 = POcp28_37+RLcp28_38;
-    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
-    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
-    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
-    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
-    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
-    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
-    VIcp28_18 = ORcp28_18+VIcp28_17;
-    VIcp28_28 = ORcp28_28+VIcp28_27;
-    VIcp28_38 = ORcp28_38+VIcp28_37;
-    RLcp28_19 = ROcp28_78*dpt[3][8];
-    RLcp28_29 = ROcp28_88*dpt[3][8];
-    RLcp28_39 = ROcp28_98*dpt[3][8];
-    POcp28_19 = POcp28_18+RLcp28_19;
-    POcp28_29 = POcp28_28+RLcp28_29;
-    POcp28_39 = POcp28_38+RLcp28_39;
-    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
-    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
-    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
-    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
-    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
-    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
-    VIcp28_19 = ORcp28_19+VIcp28_18;
-    VIcp28_29 = ORcp28_29+VIcp28_28;
-    VIcp28_39 = ORcp28_39+VIcp28_38;
-    PxF2[1] = POcp28_19;
-    PxF2[2] = POcp28_29;
-    PxF2[3] = POcp28_39;
-    RxF2[1][1] = ROcp28_19;
-    RxF2[1][2] = ROcp28_29;
-    RxF2[1][3] = ROcp28_39;
-    RxF2[2][1] = ROcp28_49;
-    RxF2[2][2] = ROcp28_59;
-    RxF2[2][3] = ROcp28_69;
-    RxF2[3][1] = ROcp28_78;
-    RxF2[3][2] = ROcp28_88;
-    RxF2[3][3] = ROcp28_98;
-    VxF2[1] = VIcp28_19;
-    VxF2[2] = VIcp28_29;
-    VxF2[3] = VIcp28_39;
-    OMxF2[1] = OMcp28_19;
-    OMxF2[2] = OMcp28_29;
-    OMxF2[3] = OMcp28_39;
-    ROcp29_25 = S4*S5;
-    ROcp29_35 = -C4*S5;
-    ROcp29_85 = -S4*C5;
-    ROcp29_95 = C4*C5;
-    ROcp29_16 = C5*C6;
-    ROcp29_26 = ROcp29_25*C6+C4*S6;
-    ROcp29_36 = ROcp29_35*C6+S4*S6;
-    ROcp29_46 = -C5*S6;
-    ROcp29_56 = -ROcp29_25*S6+C4*C6;
-    ROcp29_66 = -ROcp29_35*S6+S4*C6;
-    ROcp29_17 = ROcp29_16*C7-S5*S7;
-    ROcp29_27 = ROcp29_26*C7-ROcp29_85*S7;
-    ROcp29_37 = ROcp29_36*C7-ROcp29_95*S7;
-    ROcp29_77 = ROcp29_16*S7+S5*C7;
-    ROcp29_87 = ROcp29_26*S7+ROcp29_85*C7;
-    ROcp29_97 = ROcp29_36*S7+ROcp29_95*C7;
-    ROcp29_48 = ROcp29_46*C8+ROcp29_77*S8;
-    ROcp29_58 = ROcp29_56*C8+ROcp29_87*S8;
-    ROcp29_68 = ROcp29_66*C8+ROcp29_97*S8;
-    ROcp29_78 = -ROcp29_46*S8+ROcp29_77*C8;
-    ROcp29_88 = -ROcp29_56*S8+ROcp29_87*C8;
-    ROcp29_98 = -ROcp29_66*S8+ROcp29_97*C8;
-    ROcp29_19 = ROcp29_17*C9+ROcp29_48*S9;
-    ROcp29_29 = ROcp29_27*C9+ROcp29_58*S9;
-    ROcp29_39 = ROcp29_37*C9+ROcp29_68*S9;
-    ROcp29_49 = -ROcp29_17*S9+ROcp29_48*C9;
-    ROcp29_59 = -ROcp29_27*S9+ROcp29_58*C9;
-    ROcp29_69 = -ROcp29_37*S9+ROcp29_68*C9;
-    ROcp29_110 = ROcp29_19*C10-ROcp29_78*S10;
-    ROcp29_210 = ROcp29_29*C10-ROcp29_88*S10;
-    ROcp29_310 = ROcp29_39*C10-ROcp29_98*S10;
-    ROcp29_710 = ROcp29_19*S10+ROcp29_78*C10;
-    ROcp29_810 = ROcp29_29*S10+ROcp29_88*C10;
-    ROcp29_910 = ROcp29_39*S10+ROcp29_98*C10;
-    OMcp29_25 = qd[5]*C4;
-    OMcp29_35 = qd[5]*S4;
-    OMcp29_16 = qd[4]+qd[6]*S5;
-    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
-    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
-    RLcp29_17 = ROcp29_46*dpt[2][2];
-    RLcp29_27 = ROcp29_56*dpt[2][2];
-    RLcp29_37 = ROcp29_66*dpt[2][2];
-    POcp29_17 = q[1]+RLcp29_17;
-    POcp29_27 = q[2]+RLcp29_27;
-    POcp29_37 = q[3]+RLcp29_37;
-    OMcp29_17 = OMcp29_16+qd[7]*ROcp29_46;
-    OMcp29_27 = OMcp29_26+qd[7]*ROcp29_56;
-    OMcp29_37 = OMcp29_36+qd[7]*ROcp29_66;
-    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
-    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
-    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
-    VIcp29_17 = qd[1]+ORcp29_17;
-    VIcp29_27 = qd[2]+ORcp29_27;
-    VIcp29_37 = qd[3]+ORcp29_37;
-    RLcp29_18 = ROcp29_46*dpt[2][6];
-    RLcp29_28 = ROcp29_56*dpt[2][6];
-    RLcp29_38 = ROcp29_66*dpt[2][6];
-    POcp29_18 = POcp29_17+RLcp29_18;
-    POcp29_28 = POcp29_27+RLcp29_28;
-    POcp29_38 = POcp29_37+RLcp29_38;
-    OMcp29_18 = OMcp29_17+qd[8]*ROcp29_17;
-    OMcp29_28 = OMcp29_27+qd[8]*ROcp29_27;
-    OMcp29_38 = OMcp29_37+qd[8]*ROcp29_37;
-    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
-    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
-    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
-    VIcp29_18 = ORcp29_18+VIcp29_17;
-    VIcp29_28 = ORcp29_28+VIcp29_27;
-    VIcp29_38 = ORcp29_38+VIcp29_37;
-    RLcp29_19 = ROcp29_78*dpt[3][8];
-    RLcp29_29 = ROcp29_88*dpt[3][8];
-    RLcp29_39 = ROcp29_98*dpt[3][8];
-    POcp29_19 = POcp29_18+RLcp29_19;
-    POcp29_29 = POcp29_28+RLcp29_29;
-    POcp29_39 = POcp29_38+RLcp29_39;
-    OMcp29_19 = OMcp29_18+qd[9]*ROcp29_78;
-    OMcp29_29 = OMcp29_28+qd[9]*ROcp29_88;
-    OMcp29_39 = OMcp29_38+qd[9]*ROcp29_98;
-    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
-    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
-    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
-    VIcp29_19 = ORcp29_19+VIcp29_18;
-    VIcp29_29 = ORcp29_29+VIcp29_28;
-    VIcp29_39 = ORcp29_39+VIcp29_38;
-    RLcp29_110 = ROcp29_78*dpt[3][11];
-    RLcp29_210 = ROcp29_88*dpt[3][11];
-    RLcp29_310 = ROcp29_98*dpt[3][11];
-    POcp29_110 = POcp29_19+RLcp29_110;
-    POcp29_210 = POcp29_29+RLcp29_210;
-    POcp29_310 = POcp29_39+RLcp29_310;
-    OMcp29_110 = OMcp29_19+qd[10]*ROcp29_49;
-    OMcp29_210 = OMcp29_29+qd[10]*ROcp29_59;
-    OMcp29_310 = OMcp29_39+qd[10]*ROcp29_69;
-    ORcp29_110 = OMcp29_29*RLcp29_310-OMcp29_39*RLcp29_210;
-    ORcp29_210 = -OMcp29_19*RLcp29_310+OMcp29_39*RLcp29_110;
-    ORcp29_310 = OMcp29_19*RLcp29_210-OMcp29_29*RLcp29_110;
-    VIcp29_110 = ORcp29_110+VIcp29_19;
-    VIcp29_210 = ORcp29_210+VIcp29_29;
-    VIcp29_310 = ORcp29_310+VIcp29_39;
-    PxF3[1] = POcp29_110;
-    PxF3[2] = POcp29_210;
-    PxF3[3] = POcp29_310;
-    RxF3[1][1] = ROcp29_110;
-    RxF3[1][2] = ROcp29_210;
-    RxF3[1][3] = ROcp29_310;
-    RxF3[2][1] = ROcp29_49;
-    RxF3[2][2] = ROcp29_59;
-    RxF3[2][3] = ROcp29_69;
-    RxF3[3][1] = ROcp29_710;
-    RxF3[3][2] = ROcp29_810;
-    RxF3[3][3] = ROcp29_910;
-    VxF3[1] = VIcp29_110;
-    VxF3[2] = VIcp29_210;
-    VxF3[3] = VIcp29_310;
-    OMxF3[1] = OMcp29_110;
-    OMxF3[2] = OMcp29_210;
-    OMxF3[3] = OMcp29_310;
-    ROcp30_25 = S4*S5;
-    ROcp30_35 = -C4*S5;
-    ROcp30_85 = -S4*C5;
-    ROcp30_95 = C4*C5;
-    ROcp30_16 = C5*C6;
-    ROcp30_26 = ROcp30_25*C6+C4*S6;
-    ROcp30_36 = ROcp30_35*C6+S4*S6;
-    ROcp30_46 = -C5*S6;
-    ROcp30_56 = -ROcp30_25*S6+C4*C6;
-    ROcp30_66 = -ROcp30_35*S6+S4*C6;
-    ROcp30_17 = ROcp30_16*C7-S5*S7;
-    ROcp30_27 = ROcp30_26*C7-ROcp30_85*S7;
-    ROcp30_37 = ROcp30_36*C7-ROcp30_95*S7;
-    ROcp30_77 = ROcp30_16*S7+S5*C7;
-    ROcp30_87 = ROcp30_26*S7+ROcp30_85*C7;
-    ROcp30_97 = ROcp30_36*S7+ROcp30_95*C7;
-    ROcp30_48 = ROcp30_46*C8+ROcp30_77*S8;
-    ROcp30_58 = ROcp30_56*C8+ROcp30_87*S8;
-    ROcp30_68 = ROcp30_66*C8+ROcp30_97*S8;
-    ROcp30_78 = -ROcp30_46*S8+ROcp30_77*C8;
-    ROcp30_88 = -ROcp30_56*S8+ROcp30_87*C8;
-    ROcp30_98 = -ROcp30_66*S8+ROcp30_97*C8;
-    ROcp30_19 = ROcp30_17*C9+ROcp30_48*S9;
-    ROcp30_29 = ROcp30_27*C9+ROcp30_58*S9;
-    ROcp30_39 = ROcp30_37*C9+ROcp30_68*S9;
-    ROcp30_49 = -ROcp30_17*S9+ROcp30_48*C9;
-    ROcp30_59 = -ROcp30_27*S9+ROcp30_58*C9;
-    ROcp30_69 = -ROcp30_37*S9+ROcp30_68*C9;
-    ROcp30_110 = ROcp30_19*C10-ROcp30_78*S10;
-    ROcp30_210 = ROcp30_29*C10-ROcp30_88*S10;
-    ROcp30_310 = ROcp30_39*C10-ROcp30_98*S10;
-    ROcp30_710 = ROcp30_19*S10+ROcp30_78*C10;
-    ROcp30_810 = ROcp30_29*S10+ROcp30_88*C10;
-    ROcp30_910 = ROcp30_39*S10+ROcp30_98*C10;
-    ROcp30_411 = ROcp30_49*C11+ROcp30_710*S11;
-    ROcp30_511 = ROcp30_59*C11+ROcp30_810*S11;
-    ROcp30_611 = ROcp30_69*C11+ROcp30_910*S11;
-    ROcp30_711 = -ROcp30_49*S11+ROcp30_710*C11;
-    ROcp30_811 = -ROcp30_59*S11+ROcp30_810*C11;
-    ROcp30_911 = -ROcp30_69*S11+ROcp30_910*C11;
-    ROcp30_112 = ROcp30_110*C12-ROcp30_711*S12;
-    ROcp30_212 = ROcp30_210*C12-ROcp30_811*S12;
-    ROcp30_312 = ROcp30_310*C12-ROcp30_911*S12;
-    ROcp30_712 = ROcp30_110*S12+ROcp30_711*C12;
-    ROcp30_812 = ROcp30_210*S12+ROcp30_811*C12;
-    ROcp30_912 = ROcp30_310*S12+ROcp30_911*C12;
-    OMcp30_25 = qd[5]*C4;
-    OMcp30_35 = qd[5]*S4;
-    OMcp30_16 = qd[4]+qd[6]*S5;
-    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
-    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
-    RLcp30_17 = ROcp30_46*dpt[2][2];
-    RLcp30_27 = ROcp30_56*dpt[2][2];
-    RLcp30_37 = ROcp30_66*dpt[2][2];
-    POcp30_17 = q[1]+RLcp30_17;
-    POcp30_27 = q[2]+RLcp30_27;
-    POcp30_37 = q[3]+RLcp30_37;
-    OMcp30_17 = OMcp30_16+qd[7]*ROcp30_46;
-    OMcp30_27 = OMcp30_26+qd[7]*ROcp30_56;
-    OMcp30_37 = OMcp30_36+qd[7]*ROcp30_66;
-    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
-    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
-    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
-    VIcp30_17 = qd[1]+ORcp30_17;
-    VIcp30_27 = qd[2]+ORcp30_27;
-    VIcp30_37 = qd[3]+ORcp30_37;
-    RLcp30_18 = ROcp30_46*dpt[2][6];
-    RLcp30_28 = ROcp30_56*dpt[2][6];
-    RLcp30_38 = ROcp30_66*dpt[2][6];
-    POcp30_18 = POcp30_17+RLcp30_18;
-    POcp30_28 = POcp30_27+RLcp30_28;
-    POcp30_38 = POcp30_37+RLcp30_38;
-    OMcp30_18 = OMcp30_17+qd[8]*ROcp30_17;
-    OMcp30_28 = OMcp30_27+qd[8]*ROcp30_27;
-    OMcp30_38 = OMcp30_37+qd[8]*ROcp30_37;
-    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
-    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
-    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
-    VIcp30_18 = ORcp30_18+VIcp30_17;
-    VIcp30_28 = ORcp30_28+VIcp30_27;
-    VIcp30_38 = ORcp30_38+VIcp30_37;
-    RLcp30_19 = ROcp30_78*dpt[3][8];
-    RLcp30_29 = ROcp30_88*dpt[3][8];
-    RLcp30_39 = ROcp30_98*dpt[3][8];
-    POcp30_19 = POcp30_18+RLcp30_19;
-    POcp30_29 = POcp30_28+RLcp30_29;
-    POcp30_39 = POcp30_38+RLcp30_39;
-    OMcp30_19 = OMcp30_18+qd[9]*ROcp30_78;
-    OMcp30_29 = OMcp30_28+qd[9]*ROcp30_88;
-    OMcp30_39 = OMcp30_38+qd[9]*ROcp30_98;
-    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
-    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
-    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
-    VIcp30_19 = ORcp30_19+VIcp30_18;
-    VIcp30_29 = ORcp30_29+VIcp30_28;
-    VIcp30_39 = ORcp30_39+VIcp30_38;
-    RLcp30_110 = ROcp30_78*dpt[3][11];
-    RLcp30_210 = ROcp30_88*dpt[3][11];
-    RLcp30_310 = ROcp30_98*dpt[3][11];
-    POcp30_110 = POcp30_19+RLcp30_110;
-    POcp30_210 = POcp30_29+RLcp30_210;
-    POcp30_310 = POcp30_39+RLcp30_310;
-    OMcp30_110 = OMcp30_19+qd[10]*ROcp30_49;
-    OMcp30_210 = OMcp30_29+qd[10]*ROcp30_59;
-    OMcp30_310 = OMcp30_39+qd[10]*ROcp30_69;
-    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
-    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
-    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
-    VIcp30_110 = ORcp30_110+VIcp30_19;
-    VIcp30_210 = ORcp30_210+VIcp30_29;
-    VIcp30_310 = ORcp30_310+VIcp30_39;
-    RLcp30_111 = ROcp30_710*dpt[3][14];
-    RLcp30_211 = ROcp30_810*dpt[3][14];
-    RLcp30_311 = ROcp30_910*dpt[3][14];
-    POcp30_111 = POcp30_110+RLcp30_111;
-    POcp30_211 = POcp30_210+RLcp30_211;
-    POcp30_311 = POcp30_310+RLcp30_311;
-    OMcp30_111 = OMcp30_110+qd[11]*ROcp30_110;
-    OMcp30_211 = OMcp30_210+qd[11]*ROcp30_210;
-    OMcp30_311 = OMcp30_310+qd[11]*ROcp30_310;
-    ORcp30_111 = OMcp30_210*RLcp30_311-OMcp30_310*RLcp30_211;
-    ORcp30_211 = -OMcp30_110*RLcp30_311+OMcp30_310*RLcp30_111;
-    ORcp30_311 = OMcp30_110*RLcp30_211-OMcp30_210*RLcp30_111;
-    VIcp30_111 = ORcp30_111+VIcp30_110;
-    VIcp30_211 = ORcp30_211+VIcp30_210;
-    VIcp30_311 = ORcp30_311+VIcp30_310;
-    OMcp30_112 = OMcp30_111+qd[12]*ROcp30_411;
-    OMcp30_212 = OMcp30_211+qd[12]*ROcp30_511;
-    OMcp30_312 = OMcp30_311+qd[12]*ROcp30_611;
-    RLcp30_113 = ROcp30_112*dpt[1][20]+ROcp30_712*Dz133;
-    RLcp30_213 = ROcp30_212*dpt[1][20]+ROcp30_812*Dz133;
-    RLcp30_313 = ROcp30_312*dpt[1][20]+ROcp30_912*Dz133;
-    POcp30_113 = POcp30_111+RLcp30_113;
-    POcp30_213 = POcp30_211+RLcp30_213;
-    POcp30_313 = POcp30_311+RLcp30_313;
-    ORcp30_113 = OMcp30_212*RLcp30_313-OMcp30_312*RLcp30_213;
-    ORcp30_213 = -OMcp30_112*RLcp30_313+OMcp30_312*RLcp30_113;
-    ORcp30_313 = OMcp30_112*RLcp30_213-OMcp30_212*RLcp30_113;
-    VIcp30_113 = ORcp30_113+VIcp30_111+qd[13]*ROcp30_712;
-    VIcp30_213 = ORcp30_213+VIcp30_211+qd[13]*ROcp30_812;
-    VIcp30_313 = ORcp30_313+VIcp30_311+qd[13]*ROcp30_912;
-    PxF4[1] = POcp30_113;
-    PxF4[2] = POcp30_213;
-    PxF4[3] = POcp30_313;
-    RxF4[1][1] = ROcp30_112;
-    RxF4[1][2] = ROcp30_212;
-    RxF4[1][3] = ROcp30_312;
-    RxF4[2][1] = ROcp30_411;
-    RxF4[2][2] = ROcp30_511;
-    RxF4[2][3] = ROcp30_611;
-    RxF4[3][1] = ROcp30_712;
-    RxF4[3][2] = ROcp30_812;
-    RxF4[3][3] = ROcp30_912;
-    VxF4[1] = VIcp30_113;
-    VxF4[2] = VIcp30_213;
-    VxF4[3] = VIcp30_313;
-    OMxF4[1] = OMcp30_112;
-    OMxF4[2] = OMcp30_212;
-    OMxF4[3] = OMcp30_312;
-    ROcp31_25 = S4*S5;
-    ROcp31_35 = -C4*S5;
-    ROcp31_85 = -S4*C5;
-    ROcp31_95 = C4*C5;
-    ROcp31_16 = C5*C6;
-    ROcp31_26 = ROcp31_25*C6+C4*S6;
-    ROcp31_36 = ROcp31_35*C6+S4*S6;
-    ROcp31_46 = -C5*S6;
-    ROcp31_56 = -ROcp31_25*S6+C4*C6;
-    ROcp31_66 = -ROcp31_35*S6+S4*C6;
-    ROcp31_17 = ROcp31_16*C7-S5*S7;
-    ROcp31_27 = ROcp31_26*C7-ROcp31_85*S7;
-    ROcp31_37 = ROcp31_36*C7-ROcp31_95*S7;
-    ROcp31_77 = ROcp31_16*S7+S5*C7;
-    ROcp31_87 = ROcp31_26*S7+ROcp31_85*C7;
-    ROcp31_97 = ROcp31_36*S7+ROcp31_95*C7;
-    ROcp31_48 = ROcp31_46*C8+ROcp31_77*S8;
-    ROcp31_58 = ROcp31_56*C8+ROcp31_87*S8;
-    ROcp31_68 = ROcp31_66*C8+ROcp31_97*S8;
-    ROcp31_78 = -ROcp31_46*S8+ROcp31_77*C8;
-    ROcp31_88 = -ROcp31_56*S8+ROcp31_87*C8;
-    ROcp31_98 = -ROcp31_66*S8+ROcp31_97*C8;
-    ROcp31_19 = ROcp31_17*C9+ROcp31_48*S9;
-    ROcp31_29 = ROcp31_27*C9+ROcp31_58*S9;
-    ROcp31_39 = ROcp31_37*C9+ROcp31_68*S9;
-    ROcp31_49 = -ROcp31_17*S9+ROcp31_48*C9;
-    ROcp31_59 = -ROcp31_27*S9+ROcp31_58*C9;
-    ROcp31_69 = -ROcp31_37*S9+ROcp31_68*C9;
-    ROcp31_110 = ROcp31_19*C10-ROcp31_78*S10;
-    ROcp31_210 = ROcp31_29*C10-ROcp31_88*S10;
-    ROcp31_310 = ROcp31_39*C10-ROcp31_98*S10;
-    ROcp31_710 = ROcp31_19*S10+ROcp31_78*C10;
-    ROcp31_810 = ROcp31_29*S10+ROcp31_88*C10;
-    ROcp31_910 = ROcp31_39*S10+ROcp31_98*C10;
-    ROcp31_411 = ROcp31_49*C11+ROcp31_710*S11;
-    ROcp31_511 = ROcp31_59*C11+ROcp31_810*S11;
-    ROcp31_611 = ROcp31_69*C11+ROcp31_910*S11;
-    ROcp31_711 = -ROcp31_49*S11+ROcp31_710*C11;
-    ROcp31_811 = -ROcp31_59*S11+ROcp31_810*C11;
-    ROcp31_911 = -ROcp31_69*S11+ROcp31_910*C11;
-    ROcp31_112 = ROcp31_110*C12-ROcp31_711*S12;
-    ROcp31_212 = ROcp31_210*C12-ROcp31_811*S12;
-    ROcp31_312 = ROcp31_310*C12-ROcp31_911*S12;
-    ROcp31_712 = ROcp31_110*S12+ROcp31_711*C12;
-    ROcp31_812 = ROcp31_210*S12+ROcp31_811*C12;
-    ROcp31_912 = ROcp31_310*S12+ROcp31_911*C12;
-    OMcp31_25 = qd[5]*C4;
-    OMcp31_35 = qd[5]*S4;
-    OMcp31_16 = qd[4]+qd[6]*S5;
-    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
-    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
-    RLcp31_17 = ROcp31_46*dpt[2][2];
-    RLcp31_27 = ROcp31_56*dpt[2][2];
-    RLcp31_37 = ROcp31_66*dpt[2][2];
-    POcp31_17 = q[1]+RLcp31_17;
-    POcp31_27 = q[2]+RLcp31_27;
-    POcp31_37 = q[3]+RLcp31_37;
-    OMcp31_17 = OMcp31_16+qd[7]*ROcp31_46;
-    OMcp31_27 = OMcp31_26+qd[7]*ROcp31_56;
-    OMcp31_37 = OMcp31_36+qd[7]*ROcp31_66;
-    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
-    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
-    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
-    VIcp31_17 = qd[1]+ORcp31_17;
-    VIcp31_27 = qd[2]+ORcp31_27;
-    VIcp31_37 = qd[3]+ORcp31_37;
-    RLcp31_18 = ROcp31_46*dpt[2][6];
-    RLcp31_28 = ROcp31_56*dpt[2][6];
-    RLcp31_38 = ROcp31_66*dpt[2][6];
-    POcp31_18 = POcp31_17+RLcp31_18;
-    POcp31_28 = POcp31_27+RLcp31_28;
-    POcp31_38 = POcp31_37+RLcp31_38;
-    OMcp31_18 = OMcp31_17+qd[8]*ROcp31_17;
-    OMcp31_28 = OMcp31_27+qd[8]*ROcp31_27;
-    OMcp31_38 = OMcp31_37+qd[8]*ROcp31_37;
-    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
-    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
-    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
-    VIcp31_18 = ORcp31_18+VIcp31_17;
-    VIcp31_28 = ORcp31_28+VIcp31_27;
-    VIcp31_38 = ORcp31_38+VIcp31_37;
-    RLcp31_19 = ROcp31_78*dpt[3][8];
-    RLcp31_29 = ROcp31_88*dpt[3][8];
-    RLcp31_39 = ROcp31_98*dpt[3][8];
-    POcp31_19 = POcp31_18+RLcp31_19;
-    POcp31_29 = POcp31_28+RLcp31_29;
-    POcp31_39 = POcp31_38+RLcp31_39;
-    OMcp31_19 = OMcp31_18+qd[9]*ROcp31_78;
-    OMcp31_29 = OMcp31_28+qd[9]*ROcp31_88;
-    OMcp31_39 = OMcp31_38+qd[9]*ROcp31_98;
-    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
-    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
-    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
-    VIcp31_19 = ORcp31_19+VIcp31_18;
-    VIcp31_29 = ORcp31_29+VIcp31_28;
-    VIcp31_39 = ORcp31_39+VIcp31_38;
-    RLcp31_110 = ROcp31_78*dpt[3][11];
-    RLcp31_210 = ROcp31_88*dpt[3][11];
-    RLcp31_310 = ROcp31_98*dpt[3][11];
-    POcp31_110 = POcp31_19+RLcp31_110;
-    POcp31_210 = POcp31_29+RLcp31_210;
-    POcp31_310 = POcp31_39+RLcp31_310;
-    OMcp31_110 = OMcp31_19+qd[10]*ROcp31_49;
-    OMcp31_210 = OMcp31_29+qd[10]*ROcp31_59;
-    OMcp31_310 = OMcp31_39+qd[10]*ROcp31_69;
-    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
-    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
-    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
-    VIcp31_110 = ORcp31_110+VIcp31_19;
-    VIcp31_210 = ORcp31_210+VIcp31_29;
-    VIcp31_310 = ORcp31_310+VIcp31_39;
-    RLcp31_111 = ROcp31_710*dpt[3][14];
-    RLcp31_211 = ROcp31_810*dpt[3][14];
-    RLcp31_311 = ROcp31_910*dpt[3][14];
-    POcp31_111 = POcp31_110+RLcp31_111;
-    POcp31_211 = POcp31_210+RLcp31_211;
-    POcp31_311 = POcp31_310+RLcp31_311;
-    OMcp31_111 = OMcp31_110+qd[11]*ROcp31_110;
-    OMcp31_211 = OMcp31_210+qd[11]*ROcp31_210;
-    OMcp31_311 = OMcp31_310+qd[11]*ROcp31_310;
-    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
-    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
-    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
-    VIcp31_111 = ORcp31_111+VIcp31_110;
-    VIcp31_211 = ORcp31_211+VIcp31_210;
-    VIcp31_311 = ORcp31_311+VIcp31_310;
-    OMcp31_112 = OMcp31_111+qd[12]*ROcp31_411;
-    OMcp31_212 = OMcp31_211+qd[12]*ROcp31_511;
-    OMcp31_312 = OMcp31_311+qd[12]*ROcp31_611;
-    RLcp31_113 = ROcp31_112*dpt[1][21]+ROcp31_712*Dz143;
-    RLcp31_213 = ROcp31_212*dpt[1][21]+ROcp31_812*Dz143;
-    RLcp31_313 = ROcp31_312*dpt[1][21]+ROcp31_912*Dz143;
-    POcp31_113 = POcp31_111+RLcp31_113;
-    POcp31_213 = POcp31_211+RLcp31_213;
-    POcp31_313 = POcp31_311+RLcp31_313;
-    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
-    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
-    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
-    VIcp31_113 = ORcp31_113+VIcp31_111+qd[14]*ROcp31_712;
-    VIcp31_213 = ORcp31_213+VIcp31_211+qd[14]*ROcp31_812;
-    VIcp31_313 = ORcp31_313+VIcp31_311+qd[14]*ROcp31_912;
-    PxF5[1] = POcp31_113;
-    PxF5[2] = POcp31_213;
-    PxF5[3] = POcp31_313;
-    RxF5[1][1] = ROcp31_112;
-    RxF5[1][2] = ROcp31_212;
-    RxF5[1][3] = ROcp31_312;
-    RxF5[2][1] = ROcp31_411;
-    RxF5[2][2] = ROcp31_511;
-    RxF5[2][3] = ROcp31_611;
-    RxF5[3][1] = ROcp31_712;
-    RxF5[3][2] = ROcp31_812;
-    RxF5[3][3] = ROcp31_912;
-    VxF5[1] = VIcp31_113;
-    VxF5[2] = VIcp31_213;
-    VxF5[3] = VIcp31_313;
-    OMxF5[1] = OMcp31_112;
-    OMxF5[2] = OMcp31_212;
-    OMxF5[3] = OMcp31_312;
-    ROcp32_25 = S4*S5;
-    ROcp32_35 = -C4*S5;
-    ROcp32_85 = -S4*C5;
-    ROcp32_95 = C4*C5;
-    ROcp32_16 = C5*C6;
-    ROcp32_26 = ROcp32_25*C6+C4*S6;
-    ROcp32_36 = ROcp32_35*C6+S4*S6;
-    ROcp32_46 = -C5*S6;
-    ROcp32_56 = -ROcp32_25*S6+C4*C6;
-    ROcp32_66 = -ROcp32_35*S6+S4*C6;
-    ROcp32_115 = ROcp32_16*C15-S15*S5;
-    ROcp32_215 = ROcp32_26*C15-ROcp32_85*S15;
-    ROcp32_315 = ROcp32_36*C15-ROcp32_95*S15;
-    ROcp32_715 = ROcp32_16*S15+C15*S5;
-    ROcp32_815 = ROcp32_26*S15+ROcp32_85*C15;
-    ROcp32_915 = ROcp32_36*S15+ROcp32_95*C15;
-    ROcp32_416 = ROcp32_46*C16+ROcp32_715*S16;
-    ROcp32_516 = ROcp32_56*C16+ROcp32_815*S16;
-    ROcp32_616 = ROcp32_66*C16+ROcp32_915*S16;
-    ROcp32_716 = -ROcp32_46*S16+ROcp32_715*C16;
-    ROcp32_816 = -ROcp32_56*S16+ROcp32_815*C16;
-    ROcp32_916 = -ROcp32_66*S16+ROcp32_915*C16;
-    ROcp32_117 = ROcp32_115*C17+ROcp32_416*S17;
-    ROcp32_217 = ROcp32_215*C17+ROcp32_516*S17;
-    ROcp32_317 = ROcp32_315*C17+ROcp32_616*S17;
-    ROcp32_417 = -ROcp32_115*S17+ROcp32_416*C17;
-    ROcp32_517 = -ROcp32_215*S17+ROcp32_516*C17;
-    ROcp32_617 = -ROcp32_315*S17+ROcp32_616*C17;
-    OMcp32_25 = qd[5]*C4;
-    OMcp32_35 = qd[5]*S4;
-    OMcp32_16 = qd[4]+qd[6]*S5;
-    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
-    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
-    RLcp32_17 = ROcp32_46*dpt[2][3];
-    RLcp32_27 = ROcp32_56*dpt[2][3];
-    RLcp32_37 = ROcp32_66*dpt[2][3];
-    POcp32_17 = q[1]+RLcp32_17;
-    POcp32_27 = q[2]+RLcp32_27;
-    POcp32_37 = q[3]+RLcp32_37;
-    OMcp32_17 = OMcp32_16+qd[15]*ROcp32_46;
-    OMcp32_27 = OMcp32_26+qd[15]*ROcp32_56;
-    OMcp32_37 = OMcp32_36+qd[15]*ROcp32_66;
-    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
-    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
-    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
-    VIcp32_17 = qd[1]+ORcp32_17;
-    VIcp32_27 = qd[2]+ORcp32_27;
-    VIcp32_37 = qd[3]+ORcp32_37;
-    RLcp32_18 = ROcp32_46*dpt[2][24];
-    RLcp32_28 = ROcp32_56*dpt[2][24];
-    RLcp32_38 = ROcp32_66*dpt[2][24];
-    POcp32_18 = POcp32_17+RLcp32_18;
-    POcp32_28 = POcp32_27+RLcp32_28;
-    POcp32_38 = POcp32_37+RLcp32_38;
-    OMcp32_18 = OMcp32_17+qd[16]*ROcp32_115;
-    OMcp32_28 = OMcp32_27+qd[16]*ROcp32_215;
-    OMcp32_38 = OMcp32_37+qd[16]*ROcp32_315;
-    ORcp32_18 = OMcp32_27*RLcp32_38-OMcp32_37*RLcp32_28;
-    ORcp32_28 = -OMcp32_17*RLcp32_38+OMcp32_37*RLcp32_18;
-    ORcp32_38 = OMcp32_17*RLcp32_28-OMcp32_27*RLcp32_18;
-    VIcp32_18 = ORcp32_18+VIcp32_17;
-    VIcp32_28 = ORcp32_28+VIcp32_27;
-    VIcp32_38 = ORcp32_38+VIcp32_37;
-    RLcp32_19 = ROcp32_716*dpt[3][26];
-    RLcp32_29 = ROcp32_816*dpt[3][26];
-    RLcp32_39 = ROcp32_916*dpt[3][26];
-    POcp32_19 = POcp32_18+RLcp32_19;
-    POcp32_29 = POcp32_28+RLcp32_29;
-    POcp32_39 = POcp32_38+RLcp32_39;
-    OMcp32_19 = OMcp32_18+qd[17]*ROcp32_716;
-    OMcp32_29 = OMcp32_28+qd[17]*ROcp32_816;
-    OMcp32_39 = OMcp32_38+qd[17]*ROcp32_916;
-    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
-    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
-    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
-    VIcp32_19 = ORcp32_19+VIcp32_18;
-    VIcp32_29 = ORcp32_29+VIcp32_28;
-    VIcp32_39 = ORcp32_39+VIcp32_38;
-    PxF6[1] = POcp32_19;
-    PxF6[2] = POcp32_29;
-    PxF6[3] = POcp32_39;
-    RxF6[1][1] = ROcp32_117;
-    RxF6[1][2] = ROcp32_217;
-    RxF6[1][3] = ROcp32_317;
-    RxF6[2][1] = ROcp32_417;
-    RxF6[2][2] = ROcp32_517;
-    RxF6[2][3] = ROcp32_617;
-    RxF6[3][1] = ROcp32_716;
-    RxF6[3][2] = ROcp32_816;
-    RxF6[3][3] = ROcp32_916;
-    VxF6[1] = VIcp32_19;
-    VxF6[2] = VIcp32_29;
-    VxF6[3] = VIcp32_39;
-    OMxF6[1] = OMcp32_19;
-    OMxF6[2] = OMcp32_29;
-    OMxF6[3] = OMcp32_39;
-    ROcp33_25 = S4*S5;
-    ROcp33_35 = -C4*S5;
-    ROcp33_85 = -S4*C5;
-    ROcp33_95 = C4*C5;
-    ROcp33_16 = C5*C6;
-    ROcp33_26 = ROcp33_25*C6+C4*S6;
-    ROcp33_36 = ROcp33_35*C6+S4*S6;
-    ROcp33_46 = -C5*S6;
-    ROcp33_56 = -ROcp33_25*S6+C4*C6;
-    ROcp33_66 = -ROcp33_35*S6+S4*C6;
-    ROcp33_115 = ROcp33_16*C15-S15*S5;
-    ROcp33_215 = ROcp33_26*C15-ROcp33_85*S15;
-    ROcp33_315 = ROcp33_36*C15-ROcp33_95*S15;
-    ROcp33_715 = ROcp33_16*S15+C15*S5;
-    ROcp33_815 = ROcp33_26*S15+ROcp33_85*C15;
-    ROcp33_915 = ROcp33_36*S15+ROcp33_95*C15;
-    ROcp33_416 = ROcp33_46*C16+ROcp33_715*S16;
-    ROcp33_516 = ROcp33_56*C16+ROcp33_815*S16;
-    ROcp33_616 = ROcp33_66*C16+ROcp33_915*S16;
-    ROcp33_716 = -ROcp33_46*S16+ROcp33_715*C16;
-    ROcp33_816 = -ROcp33_56*S16+ROcp33_815*C16;
-    ROcp33_916 = -ROcp33_66*S16+ROcp33_915*C16;
-    ROcp33_117 = ROcp33_115*C17+ROcp33_416*S17;
-    ROcp33_217 = ROcp33_215*C17+ROcp33_516*S17;
-    ROcp33_317 = ROcp33_315*C17+ROcp33_616*S17;
-    ROcp33_417 = -ROcp33_115*S17+ROcp33_416*C17;
-    ROcp33_517 = -ROcp33_215*S17+ROcp33_516*C17;
-    ROcp33_617 = -ROcp33_315*S17+ROcp33_616*C17;
-    ROcp33_118 = ROcp33_117*C18-ROcp33_716*S18;
-    ROcp33_218 = ROcp33_217*C18-ROcp33_816*S18;
-    ROcp33_318 = ROcp33_317*C18-ROcp33_916*S18;
-    ROcp33_718 = ROcp33_117*S18+ROcp33_716*C18;
-    ROcp33_818 = ROcp33_217*S18+ROcp33_816*C18;
-    ROcp33_918 = ROcp33_317*S18+ROcp33_916*C18;
-    OMcp33_25 = qd[5]*C4;
-    OMcp33_35 = qd[5]*S4;
-    OMcp33_16 = qd[4]+qd[6]*S5;
-    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
-    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
-    RLcp33_17 = ROcp33_46*dpt[2][3];
-    RLcp33_27 = ROcp33_56*dpt[2][3];
-    RLcp33_37 = ROcp33_66*dpt[2][3];
-    POcp33_17 = q[1]+RLcp33_17;
-    POcp33_27 = q[2]+RLcp33_27;
-    POcp33_37 = q[3]+RLcp33_37;
-    OMcp33_17 = OMcp33_16+qd[15]*ROcp33_46;
-    OMcp33_27 = OMcp33_26+qd[15]*ROcp33_56;
-    OMcp33_37 = OMcp33_36+qd[15]*ROcp33_66;
-    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
-    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
-    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
-    VIcp33_17 = qd[1]+ORcp33_17;
-    VIcp33_27 = qd[2]+ORcp33_27;
-    VIcp33_37 = qd[3]+ORcp33_37;
-    RLcp33_18 = ROcp33_46*dpt[2][24];
-    RLcp33_28 = ROcp33_56*dpt[2][24];
-    RLcp33_38 = ROcp33_66*dpt[2][24];
-    POcp33_18 = POcp33_17+RLcp33_18;
-    POcp33_28 = POcp33_27+RLcp33_28;
-    POcp33_38 = POcp33_37+RLcp33_38;
-    OMcp33_18 = OMcp33_17+qd[16]*ROcp33_115;
-    OMcp33_28 = OMcp33_27+qd[16]*ROcp33_215;
-    OMcp33_38 = OMcp33_37+qd[16]*ROcp33_315;
-    ORcp33_18 = OMcp33_27*RLcp33_38-OMcp33_37*RLcp33_28;
-    ORcp33_28 = -OMcp33_17*RLcp33_38+OMcp33_37*RLcp33_18;
-    ORcp33_38 = OMcp33_17*RLcp33_28-OMcp33_27*RLcp33_18;
-    VIcp33_18 = ORcp33_18+VIcp33_17;
-    VIcp33_28 = ORcp33_28+VIcp33_27;
-    VIcp33_38 = ORcp33_38+VIcp33_37;
-    RLcp33_19 = ROcp33_716*dpt[3][26];
-    RLcp33_29 = ROcp33_816*dpt[3][26];
-    RLcp33_39 = ROcp33_916*dpt[3][26];
-    POcp33_19 = POcp33_18+RLcp33_19;
-    POcp33_29 = POcp33_28+RLcp33_29;
-    POcp33_39 = POcp33_38+RLcp33_39;
-    OMcp33_19 = OMcp33_18+qd[17]*ROcp33_716;
-    OMcp33_29 = OMcp33_28+qd[17]*ROcp33_816;
-    OMcp33_39 = OMcp33_38+qd[17]*ROcp33_916;
-    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
-    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
-    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
-    VIcp33_19 = ORcp33_19+VIcp33_18;
-    VIcp33_29 = ORcp33_29+VIcp33_28;
-    VIcp33_39 = ORcp33_39+VIcp33_38;
-    RLcp33_110 = ROcp33_716*dpt[3][29];
-    RLcp33_210 = ROcp33_816*dpt[3][29];
-    RLcp33_310 = ROcp33_916*dpt[3][29];
-    POcp33_110 = POcp33_19+RLcp33_110;
-    POcp33_210 = POcp33_29+RLcp33_210;
-    POcp33_310 = POcp33_39+RLcp33_310;
-    OMcp33_110 = OMcp33_19+qd[18]*ROcp33_417;
-    OMcp33_210 = OMcp33_29+qd[18]*ROcp33_517;
-    OMcp33_310 = OMcp33_39+qd[18]*ROcp33_617;
-    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
-    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
-    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
-    VIcp33_110 = ORcp33_110+VIcp33_19;
-    VIcp33_210 = ORcp33_210+VIcp33_29;
-    VIcp33_310 = ORcp33_310+VIcp33_39;
-    PxF7[1] = POcp33_110;
-    PxF7[2] = POcp33_210;
-    PxF7[3] = POcp33_310;
-    RxF7[1][1] = ROcp33_118;
-    RxF7[1][2] = ROcp33_218;
-    RxF7[1][3] = ROcp33_318;
-    RxF7[2][1] = ROcp33_417;
-    RxF7[2][2] = ROcp33_517;
-    RxF7[2][3] = ROcp33_617;
-    RxF7[3][1] = ROcp33_718;
-    RxF7[3][2] = ROcp33_818;
-    RxF7[3][3] = ROcp33_918;
-    VxF7[1] = VIcp33_110;
-    VxF7[2] = VIcp33_210;
-    VxF7[3] = VIcp33_310;
-    OMxF7[1] = OMcp33_110;
-    OMxF7[2] = OMcp33_210;
-    OMxF7[3] = OMcp33_310;
-    ROcp34_25 = S4*S5;
-    ROcp34_35 = -C4*S5;
-    ROcp34_85 = -S4*C5;
-    ROcp34_95 = C4*C5;
-    ROcp34_16 = C5*C6;
-    ROcp34_26 = ROcp34_25*C6+C4*S6;
-    ROcp34_36 = ROcp34_35*C6+S4*S6;
-    ROcp34_46 = -C5*S6;
-    ROcp34_56 = -ROcp34_25*S6+C4*C6;
-    ROcp34_66 = -ROcp34_35*S6+S4*C6;
-    ROcp34_115 = ROcp34_16*C15-S15*S5;
-    ROcp34_215 = ROcp34_26*C15-ROcp34_85*S15;
-    ROcp34_315 = ROcp34_36*C15-ROcp34_95*S15;
-    ROcp34_715 = ROcp34_16*S15+C15*S5;
-    ROcp34_815 = ROcp34_26*S15+ROcp34_85*C15;
-    ROcp34_915 = ROcp34_36*S15+ROcp34_95*C15;
-    ROcp34_416 = ROcp34_46*C16+ROcp34_715*S16;
-    ROcp34_516 = ROcp34_56*C16+ROcp34_815*S16;
-    ROcp34_616 = ROcp34_66*C16+ROcp34_915*S16;
-    ROcp34_716 = -ROcp34_46*S16+ROcp34_715*C16;
-    ROcp34_816 = -ROcp34_56*S16+ROcp34_815*C16;
-    ROcp34_916 = -ROcp34_66*S16+ROcp34_915*C16;
-    ROcp34_117 = ROcp34_115*C17+ROcp34_416*S17;
-    ROcp34_217 = ROcp34_215*C17+ROcp34_516*S17;
-    ROcp34_317 = ROcp34_315*C17+ROcp34_616*S17;
-    ROcp34_417 = -ROcp34_115*S17+ROcp34_416*C17;
-    ROcp34_517 = -ROcp34_215*S17+ROcp34_516*C17;
-    ROcp34_617 = -ROcp34_315*S17+ROcp34_616*C17;
-    ROcp34_118 = ROcp34_117*C18-ROcp34_716*S18;
-    ROcp34_218 = ROcp34_217*C18-ROcp34_816*S18;
-    ROcp34_318 = ROcp34_317*C18-ROcp34_916*S18;
-    ROcp34_718 = ROcp34_117*S18+ROcp34_716*C18;
-    ROcp34_818 = ROcp34_217*S18+ROcp34_816*C18;
-    ROcp34_918 = ROcp34_317*S18+ROcp34_916*C18;
-    ROcp34_419 = ROcp34_417*C19+ROcp34_718*S19;
-    ROcp34_519 = ROcp34_517*C19+ROcp34_818*S19;
-    ROcp34_619 = ROcp34_617*C19+ROcp34_918*S19;
-    ROcp34_719 = -ROcp34_417*S19+ROcp34_718*C19;
-    ROcp34_819 = -ROcp34_517*S19+ROcp34_818*C19;
-    ROcp34_919 = -ROcp34_617*S19+ROcp34_918*C19;
-    ROcp34_120 = ROcp34_118*C20-ROcp34_719*S20;
-    ROcp34_220 = ROcp34_218*C20-ROcp34_819*S20;
-    ROcp34_320 = ROcp34_318*C20-ROcp34_919*S20;
-    ROcp34_720 = ROcp34_118*S20+ROcp34_719*C20;
-    ROcp34_820 = ROcp34_218*S20+ROcp34_819*C20;
-    ROcp34_920 = ROcp34_318*S20+ROcp34_919*C20;
-    OMcp34_25 = qd[5]*C4;
-    OMcp34_35 = qd[5]*S4;
-    OMcp34_16 = qd[4]+qd[6]*S5;
-    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
-    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
-    RLcp34_17 = ROcp34_46*dpt[2][3];
-    RLcp34_27 = ROcp34_56*dpt[2][3];
-    RLcp34_37 = ROcp34_66*dpt[2][3];
-    POcp34_17 = q[1]+RLcp34_17;
-    POcp34_27 = q[2]+RLcp34_27;
-    POcp34_37 = q[3]+RLcp34_37;
-    OMcp34_17 = OMcp34_16+qd[15]*ROcp34_46;
-    OMcp34_27 = OMcp34_26+qd[15]*ROcp34_56;
-    OMcp34_37 = OMcp34_36+qd[15]*ROcp34_66;
-    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
-    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
-    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
-    VIcp34_17 = qd[1]+ORcp34_17;
-    VIcp34_27 = qd[2]+ORcp34_27;
-    VIcp34_37 = qd[3]+ORcp34_37;
-    RLcp34_18 = ROcp34_46*dpt[2][24];
-    RLcp34_28 = ROcp34_56*dpt[2][24];
-    RLcp34_38 = ROcp34_66*dpt[2][24];
-    POcp34_18 = POcp34_17+RLcp34_18;
-    POcp34_28 = POcp34_27+RLcp34_28;
-    POcp34_38 = POcp34_37+RLcp34_38;
-    OMcp34_18 = OMcp34_17+qd[16]*ROcp34_115;
-    OMcp34_28 = OMcp34_27+qd[16]*ROcp34_215;
-    OMcp34_38 = OMcp34_37+qd[16]*ROcp34_315;
-    ORcp34_18 = OMcp34_27*RLcp34_38-OMcp34_37*RLcp34_28;
-    ORcp34_28 = -OMcp34_17*RLcp34_38+OMcp34_37*RLcp34_18;
-    ORcp34_38 = OMcp34_17*RLcp34_28-OMcp34_27*RLcp34_18;
-    VIcp34_18 = ORcp34_18+VIcp34_17;
-    VIcp34_28 = ORcp34_28+VIcp34_27;
-    VIcp34_38 = ORcp34_38+VIcp34_37;
-    RLcp34_19 = ROcp34_716*dpt[3][26];
-    RLcp34_29 = ROcp34_816*dpt[3][26];
-    RLcp34_39 = ROcp34_916*dpt[3][26];
-    POcp34_19 = POcp34_18+RLcp34_19;
-    POcp34_29 = POcp34_28+RLcp34_29;
-    POcp34_39 = POcp34_38+RLcp34_39;
-    OMcp34_19 = OMcp34_18+qd[17]*ROcp34_716;
-    OMcp34_29 = OMcp34_28+qd[17]*ROcp34_816;
-    OMcp34_39 = OMcp34_38+qd[17]*ROcp34_916;
-    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
-    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
-    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
-    VIcp34_19 = ORcp34_19+VIcp34_18;
-    VIcp34_29 = ORcp34_29+VIcp34_28;
-    VIcp34_39 = ORcp34_39+VIcp34_38;
-    RLcp34_110 = ROcp34_716*dpt[3][29];
-    RLcp34_210 = ROcp34_816*dpt[3][29];
-    RLcp34_310 = ROcp34_916*dpt[3][29];
-    POcp34_110 = POcp34_19+RLcp34_110;
-    POcp34_210 = POcp34_29+RLcp34_210;
-    POcp34_310 = POcp34_39+RLcp34_310;
-    OMcp34_110 = OMcp34_19+qd[18]*ROcp34_417;
-    OMcp34_210 = OMcp34_29+qd[18]*ROcp34_517;
-    OMcp34_310 = OMcp34_39+qd[18]*ROcp34_617;
-    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
-    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
-    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
-    VIcp34_110 = ORcp34_110+VIcp34_19;
-    VIcp34_210 = ORcp34_210+VIcp34_29;
-    VIcp34_310 = ORcp34_310+VIcp34_39;
-    RLcp34_111 = ROcp34_718*dpt[3][32];
-    RLcp34_211 = ROcp34_818*dpt[3][32];
-    RLcp34_311 = ROcp34_918*dpt[3][32];
-    POcp34_111 = POcp34_110+RLcp34_111;
-    POcp34_211 = POcp34_210+RLcp34_211;
-    POcp34_311 = POcp34_310+RLcp34_311;
-    OMcp34_111 = OMcp34_110+qd[19]*ROcp34_118;
-    OMcp34_211 = OMcp34_210+qd[19]*ROcp34_218;
-    OMcp34_311 = OMcp34_310+qd[19]*ROcp34_318;
-    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
-    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
-    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
-    VIcp34_111 = ORcp34_111+VIcp34_110;
-    VIcp34_211 = ORcp34_211+VIcp34_210;
-    VIcp34_311 = ORcp34_311+VIcp34_310;
-    OMcp34_112 = OMcp34_111+qd[20]*ROcp34_419;
-    OMcp34_212 = OMcp34_211+qd[20]*ROcp34_519;
-    OMcp34_312 = OMcp34_311+qd[20]*ROcp34_619;
-    RLcp34_113 = ROcp34_120*dpt[1][38]+ROcp34_720*Dz213;
-    RLcp34_213 = ROcp34_220*dpt[1][38]+ROcp34_820*Dz213;
-    RLcp34_313 = ROcp34_320*dpt[1][38]+ROcp34_920*Dz213;
-    POcp34_113 = POcp34_111+RLcp34_113;
-    POcp34_213 = POcp34_211+RLcp34_213;
-    POcp34_313 = POcp34_311+RLcp34_313;
-    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
-    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
-    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
-    VIcp34_113 = ORcp34_113+VIcp34_111+qd[21]*ROcp34_720;
-    VIcp34_213 = ORcp34_213+VIcp34_211+qd[21]*ROcp34_820;
-    VIcp34_313 = ORcp34_313+VIcp34_311+qd[21]*ROcp34_920;
-    PxF8[1] = POcp34_113;
-    PxF8[2] = POcp34_213;
-    PxF8[3] = POcp34_313;
-    RxF8[1][1] = ROcp34_120;
-    RxF8[1][2] = ROcp34_220;
-    RxF8[1][3] = ROcp34_320;
-    RxF8[2][1] = ROcp34_419;
-    RxF8[2][2] = ROcp34_519;
-    RxF8[2][3] = ROcp34_619;
-    RxF8[3][1] = ROcp34_720;
-    RxF8[3][2] = ROcp34_820;
-    RxF8[3][3] = ROcp34_920;
-    VxF8[1] = VIcp34_113;
-    VxF8[2] = VIcp34_213;
-    VxF8[3] = VIcp34_313;
-    OMxF8[1] = OMcp34_112;
-    OMxF8[2] = OMcp34_212;
-    OMxF8[3] = OMcp34_312;
-    ROcp35_25 = S4*S5;
-    ROcp35_35 = -C4*S5;
-    ROcp35_85 = -S4*C5;
-    ROcp35_95 = C4*C5;
-    ROcp35_16 = C5*C6;
-    ROcp35_26 = ROcp35_25*C6+C4*S6;
-    ROcp35_36 = ROcp35_35*C6+S4*S6;
-    ROcp35_46 = -C5*S6;
-    ROcp35_56 = -ROcp35_25*S6+C4*C6;
-    ROcp35_66 = -ROcp35_35*S6+S4*C6;
-    ROcp35_115 = ROcp35_16*C15-S15*S5;
-    ROcp35_215 = ROcp35_26*C15-ROcp35_85*S15;
-    ROcp35_315 = ROcp35_36*C15-ROcp35_95*S15;
-    ROcp35_715 = ROcp35_16*S15+C15*S5;
-    ROcp35_815 = ROcp35_26*S15+ROcp35_85*C15;
-    ROcp35_915 = ROcp35_36*S15+ROcp35_95*C15;
-    ROcp35_416 = ROcp35_46*C16+ROcp35_715*S16;
-    ROcp35_516 = ROcp35_56*C16+ROcp35_815*S16;
-    ROcp35_616 = ROcp35_66*C16+ROcp35_915*S16;
-    ROcp35_716 = -ROcp35_46*S16+ROcp35_715*C16;
-    ROcp35_816 = -ROcp35_56*S16+ROcp35_815*C16;
-    ROcp35_916 = -ROcp35_66*S16+ROcp35_915*C16;
-    ROcp35_117 = ROcp35_115*C17+ROcp35_416*S17;
-    ROcp35_217 = ROcp35_215*C17+ROcp35_516*S17;
-    ROcp35_317 = ROcp35_315*C17+ROcp35_616*S17;
-    ROcp35_417 = -ROcp35_115*S17+ROcp35_416*C17;
-    ROcp35_517 = -ROcp35_215*S17+ROcp35_516*C17;
-    ROcp35_617 = -ROcp35_315*S17+ROcp35_616*C17;
-    ROcp35_118 = ROcp35_117*C18-ROcp35_716*S18;
-    ROcp35_218 = ROcp35_217*C18-ROcp35_816*S18;
-    ROcp35_318 = ROcp35_317*C18-ROcp35_916*S18;
-    ROcp35_718 = ROcp35_117*S18+ROcp35_716*C18;
-    ROcp35_818 = ROcp35_217*S18+ROcp35_816*C18;
-    ROcp35_918 = ROcp35_317*S18+ROcp35_916*C18;
-    ROcp35_419 = ROcp35_417*C19+ROcp35_718*S19;
-    ROcp35_519 = ROcp35_517*C19+ROcp35_818*S19;
-    ROcp35_619 = ROcp35_617*C19+ROcp35_918*S19;
-    ROcp35_719 = -ROcp35_417*S19+ROcp35_718*C19;
-    ROcp35_819 = -ROcp35_517*S19+ROcp35_818*C19;
-    ROcp35_919 = -ROcp35_617*S19+ROcp35_918*C19;
-    ROcp35_120 = ROcp35_118*C20-ROcp35_719*S20;
-    ROcp35_220 = ROcp35_218*C20-ROcp35_819*S20;
-    ROcp35_320 = ROcp35_318*C20-ROcp35_919*S20;
-    ROcp35_720 = ROcp35_118*S20+ROcp35_719*C20;
-    ROcp35_820 = ROcp35_218*S20+ROcp35_819*C20;
-    ROcp35_920 = ROcp35_318*S20+ROcp35_919*C20;
-    OMcp35_25 = qd[5]*C4;
-    OMcp35_35 = qd[5]*S4;
-    OMcp35_16 = qd[4]+qd[6]*S5;
-    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
-    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
-    RLcp35_17 = ROcp35_46*dpt[2][3];
-    RLcp35_27 = ROcp35_56*dpt[2][3];
-    RLcp35_37 = ROcp35_66*dpt[2][3];
-    POcp35_17 = q[1]+RLcp35_17;
-    POcp35_27 = q[2]+RLcp35_27;
-    POcp35_37 = q[3]+RLcp35_37;
-    OMcp35_17 = OMcp35_16+qd[15]*ROcp35_46;
-    OMcp35_27 = OMcp35_26+qd[15]*ROcp35_56;
-    OMcp35_37 = OMcp35_36+qd[15]*ROcp35_66;
-    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
-    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
-    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
-    VIcp35_17 = qd[1]+ORcp35_17;
-    VIcp35_27 = qd[2]+ORcp35_27;
-    VIcp35_37 = qd[3]+ORcp35_37;
-    RLcp35_18 = ROcp35_46*dpt[2][24];
-    RLcp35_28 = ROcp35_56*dpt[2][24];
-    RLcp35_38 = ROcp35_66*dpt[2][24];
-    POcp35_18 = POcp35_17+RLcp35_18;
-    POcp35_28 = POcp35_27+RLcp35_28;
-    POcp35_38 = POcp35_37+RLcp35_38;
-    OMcp35_18 = OMcp35_17+qd[16]*ROcp35_115;
-    OMcp35_28 = OMcp35_27+qd[16]*ROcp35_215;
-    OMcp35_38 = OMcp35_37+qd[16]*ROcp35_315;
-    ORcp35_18 = OMcp35_27*RLcp35_38-OMcp35_37*RLcp35_28;
-    ORcp35_28 = -OMcp35_17*RLcp35_38+OMcp35_37*RLcp35_18;
-    ORcp35_38 = OMcp35_17*RLcp35_28-OMcp35_27*RLcp35_18;
-    VIcp35_18 = ORcp35_18+VIcp35_17;
-    VIcp35_28 = ORcp35_28+VIcp35_27;
-    VIcp35_38 = ORcp35_38+VIcp35_37;
-    RLcp35_19 = ROcp35_716*dpt[3][26];
-    RLcp35_29 = ROcp35_816*dpt[3][26];
-    RLcp35_39 = ROcp35_916*dpt[3][26];
-    POcp35_19 = POcp35_18+RLcp35_19;
-    POcp35_29 = POcp35_28+RLcp35_29;
-    POcp35_39 = POcp35_38+RLcp35_39;
-    OMcp35_19 = OMcp35_18+qd[17]*ROcp35_716;
-    OMcp35_29 = OMcp35_28+qd[17]*ROcp35_816;
-    OMcp35_39 = OMcp35_38+qd[17]*ROcp35_916;
-    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
-    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
-    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
-    VIcp35_19 = ORcp35_19+VIcp35_18;
-    VIcp35_29 = ORcp35_29+VIcp35_28;
-    VIcp35_39 = ORcp35_39+VIcp35_38;
-    RLcp35_110 = ROcp35_716*dpt[3][29];
-    RLcp35_210 = ROcp35_816*dpt[3][29];
-    RLcp35_310 = ROcp35_916*dpt[3][29];
-    POcp35_110 = POcp35_19+RLcp35_110;
-    POcp35_210 = POcp35_29+RLcp35_210;
-    POcp35_310 = POcp35_39+RLcp35_310;
-    OMcp35_110 = OMcp35_19+qd[18]*ROcp35_417;
-    OMcp35_210 = OMcp35_29+qd[18]*ROcp35_517;
-    OMcp35_310 = OMcp35_39+qd[18]*ROcp35_617;
-    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
-    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
-    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
-    VIcp35_110 = ORcp35_110+VIcp35_19;
-    VIcp35_210 = ORcp35_210+VIcp35_29;
-    VIcp35_310 = ORcp35_310+VIcp35_39;
-    RLcp35_111 = ROcp35_718*dpt[3][32];
-    RLcp35_211 = ROcp35_818*dpt[3][32];
-    RLcp35_311 = ROcp35_918*dpt[3][32];
-    POcp35_111 = POcp35_110+RLcp35_111;
-    POcp35_211 = POcp35_210+RLcp35_211;
-    POcp35_311 = POcp35_310+RLcp35_311;
-    OMcp35_111 = OMcp35_110+qd[19]*ROcp35_118;
-    OMcp35_211 = OMcp35_210+qd[19]*ROcp35_218;
-    OMcp35_311 = OMcp35_310+qd[19]*ROcp35_318;
-    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
-    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
-    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
-    VIcp35_111 = ORcp35_111+VIcp35_110;
-    VIcp35_211 = ORcp35_211+VIcp35_210;
-    VIcp35_311 = ORcp35_311+VIcp35_310;
-    OMcp35_112 = OMcp35_111+qd[20]*ROcp35_419;
-    OMcp35_212 = OMcp35_211+qd[20]*ROcp35_519;
-    OMcp35_312 = OMcp35_311+qd[20]*ROcp35_619;
-    RLcp35_113 = ROcp35_120*dpt[1][39]+ROcp35_720*Dz223;
-    RLcp35_213 = ROcp35_220*dpt[1][39]+ROcp35_820*Dz223;
-    RLcp35_313 = ROcp35_320*dpt[1][39]+ROcp35_920*Dz223;
-    POcp35_113 = POcp35_111+RLcp35_113;
-    POcp35_213 = POcp35_211+RLcp35_213;
-    POcp35_313 = POcp35_311+RLcp35_313;
-    ORcp35_113 = OMcp35_212*RLcp35_313-OMcp35_312*RLcp35_213;
-    ORcp35_213 = -OMcp35_112*RLcp35_313+OMcp35_312*RLcp35_113;
-    ORcp35_313 = OMcp35_112*RLcp35_213-OMcp35_212*RLcp35_113;
-    VIcp35_113 = ORcp35_113+VIcp35_111+qd[22]*ROcp35_720;
-    VIcp35_213 = ORcp35_213+VIcp35_211+qd[22]*ROcp35_820;
-    VIcp35_313 = ORcp35_313+VIcp35_311+qd[22]*ROcp35_920;
-    PxF9[1] = POcp35_113;
-    PxF9[2] = POcp35_213;
-    PxF9[3] = POcp35_313;
-    RxF9[1][1] = ROcp35_120;
-    RxF9[1][2] = ROcp35_220;
-    RxF9[1][3] = ROcp35_320;
-    RxF9[2][1] = ROcp35_419;
-    RxF9[2][2] = ROcp35_519;
-    RxF9[2][3] = ROcp35_619;
-    RxF9[3][1] = ROcp35_720;
-    RxF9[3][2] = ROcp35_820;
-    RxF9[3][3] = ROcp35_920;
-    VxF9[1] = VIcp35_113;
-    VxF9[2] = VIcp35_213;
-    VxF9[3] = VIcp35_313;
-    OMxF9[1] = OMcp35_112;
-    OMxF9[2] = OMcp35_212;
-    OMxF9[3] = OMcp35_312;
-    ROcp36_25 = S4*S5;
-    ROcp36_35 = -C4*S5;
-    ROcp36_85 = -S4*C5;
-    ROcp36_95 = C4*C5;
-    ROcp36_16 = C5*C6;
-    ROcp36_26 = ROcp36_25*C6+C4*S6;
-    ROcp36_36 = ROcp36_35*C6+S4*S6;
-    ROcp36_46 = -C5*S6;
-    ROcp36_56 = -ROcp36_25*S6+C4*C6;
-    ROcp36_66 = -ROcp36_35*S6+S4*C6;
-    ROcp36_423 = ROcp36_46*C23+S23*S5;
-    ROcp36_523 = ROcp36_56*C23+ROcp36_85*S23;
-    ROcp36_623 = ROcp36_66*C23+ROcp36_95*S23;
-    ROcp36_723 = -ROcp36_46*S23+C23*S5;
-    ROcp36_823 = -ROcp36_56*S23+ROcp36_85*C23;
-    ROcp36_923 = -ROcp36_66*S23+ROcp36_95*C23;
-    ROcp36_124 = ROcp36_16*C24-ROcp36_723*S24;
-    ROcp36_224 = ROcp36_26*C24-ROcp36_823*S24;
-    ROcp36_324 = ROcp36_36*C24-ROcp36_923*S24;
-    ROcp36_724 = ROcp36_16*S24+ROcp36_723*C24;
-    ROcp36_824 = ROcp36_26*S24+ROcp36_823*C24;
-    ROcp36_924 = ROcp36_36*S24+ROcp36_923*C24;
-    ROcp36_125 = ROcp36_124*C25+ROcp36_423*S25;
-    ROcp36_225 = ROcp36_224*C25+ROcp36_523*S25;
-    ROcp36_325 = ROcp36_324*C25+ROcp36_623*S25;
-    ROcp36_425 = -ROcp36_124*S25+ROcp36_423*C25;
-    ROcp36_525 = -ROcp36_224*S25+ROcp36_523*C25;
-    ROcp36_625 = -ROcp36_324*S25+ROcp36_623*C25;
-    OMcp36_25 = qd[5]*C4;
-    OMcp36_35 = qd[5]*S4;
-    OMcp36_16 = qd[4]+qd[6]*S5;
-    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
-    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
-    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
-    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
-    POcp36_17 = q[1]+RLcp36_17;
-    POcp36_27 = q[2]+RLcp36_27;
-    POcp36_37 = q[3]+RLcp36_37;
-    OMcp36_17 = OMcp36_16+qd[23]*ROcp36_16;
-    OMcp36_27 = OMcp36_26+qd[23]*ROcp36_26;
-    OMcp36_37 = OMcp36_36+qd[23]*ROcp36_36;
-    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
-    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
-    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
-    VIcp36_17 = qd[1]+ORcp36_17;
-    VIcp36_27 = qd[2]+ORcp36_27;
-    VIcp36_37 = qd[3]+ORcp36_37;
-    OMcp36_18 = OMcp36_17+qd[24]*ROcp36_423;
-    OMcp36_28 = OMcp36_27+qd[24]*ROcp36_523;
-    OMcp36_38 = OMcp36_37+qd[24]*ROcp36_623;
-    RLcp36_19 = ROcp36_724*dpt[3][44];
-    RLcp36_29 = ROcp36_824*dpt[3][44];
-    RLcp36_39 = ROcp36_924*dpt[3][44];
-    POcp36_19 = POcp36_17+RLcp36_19;
-    POcp36_29 = POcp36_27+RLcp36_29;
-    POcp36_39 = POcp36_37+RLcp36_39;
-    OMcp36_19 = OMcp36_18+qd[25]*ROcp36_724;
-    OMcp36_29 = OMcp36_28+qd[25]*ROcp36_824;
-    OMcp36_39 = OMcp36_38+qd[25]*ROcp36_924;
-    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
-    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
-    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
-    VIcp36_19 = ORcp36_19+VIcp36_17;
-    VIcp36_29 = ORcp36_29+VIcp36_27;
-    VIcp36_39 = ORcp36_39+VIcp36_37;
-    PxF10[1] = POcp36_19;
-    PxF10[2] = POcp36_29;
-    PxF10[3] = POcp36_39;
-    RxF10[1][1] = ROcp36_125;
-    RxF10[1][2] = ROcp36_225;
-    RxF10[1][3] = ROcp36_325;
-    RxF10[2][1] = ROcp36_425;
-    RxF10[2][2] = ROcp36_525;
-    RxF10[2][3] = ROcp36_625;
-    RxF10[3][1] = ROcp36_724;
-    RxF10[3][2] = ROcp36_824;
-    RxF10[3][3] = ROcp36_924;
-    VxF10[1] = VIcp36_19;
-    VxF10[2] = VIcp36_29;
-    VxF10[3] = VIcp36_39;
-    OMxF10[1] = OMcp36_19;
-    OMxF10[2] = OMcp36_29;
-    OMxF10[3] = OMcp36_39;
-    ROcp37_25 = S4*S5;
-    ROcp37_35 = -C4*S5;
-    ROcp37_85 = -S4*C5;
-    ROcp37_95 = C4*C5;
-    ROcp37_16 = C5*C6;
-    ROcp37_26 = ROcp37_25*C6+C4*S6;
-    ROcp37_36 = ROcp37_35*C6+S4*S6;
-    ROcp37_46 = -C5*S6;
-    ROcp37_56 = -ROcp37_25*S6+C4*C6;
-    ROcp37_66 = -ROcp37_35*S6+S4*C6;
-    ROcp37_423 = ROcp37_46*C23+S23*S5;
-    ROcp37_523 = ROcp37_56*C23+ROcp37_85*S23;
-    ROcp37_623 = ROcp37_66*C23+ROcp37_95*S23;
-    ROcp37_723 = -ROcp37_46*S23+C23*S5;
-    ROcp37_823 = -ROcp37_56*S23+ROcp37_85*C23;
-    ROcp37_923 = -ROcp37_66*S23+ROcp37_95*C23;
-    ROcp37_124 = ROcp37_16*C24-ROcp37_723*S24;
-    ROcp37_224 = ROcp37_26*C24-ROcp37_823*S24;
-    ROcp37_324 = ROcp37_36*C24-ROcp37_923*S24;
-    ROcp37_724 = ROcp37_16*S24+ROcp37_723*C24;
-    ROcp37_824 = ROcp37_26*S24+ROcp37_823*C24;
-    ROcp37_924 = ROcp37_36*S24+ROcp37_923*C24;
-    ROcp37_125 = ROcp37_124*C25+ROcp37_423*S25;
-    ROcp37_225 = ROcp37_224*C25+ROcp37_523*S25;
-    ROcp37_325 = ROcp37_324*C25+ROcp37_623*S25;
-    ROcp37_425 = -ROcp37_124*S25+ROcp37_423*C25;
-    ROcp37_525 = -ROcp37_224*S25+ROcp37_523*C25;
-    ROcp37_625 = -ROcp37_324*S25+ROcp37_623*C25;
-    ROcp37_126 = ROcp37_125*C26-ROcp37_724*S26;
-    ROcp37_226 = ROcp37_225*C26-ROcp37_824*S26;
-    ROcp37_326 = ROcp37_325*C26-ROcp37_924*S26;
-    ROcp37_726 = ROcp37_125*S26+ROcp37_724*C26;
-    ROcp37_826 = ROcp37_225*S26+ROcp37_824*C26;
-    ROcp37_926 = ROcp37_325*S26+ROcp37_924*C26;
-    ROcp37_427 = ROcp37_425*C27+ROcp37_726*S27;
-    ROcp37_527 = ROcp37_525*C27+ROcp37_826*S27;
-    ROcp37_627 = ROcp37_625*C27+ROcp37_926*S27;
-    ROcp37_727 = -ROcp37_425*S27+ROcp37_726*C27;
-    ROcp37_827 = -ROcp37_525*S27+ROcp37_826*C27;
-    ROcp37_927 = -ROcp37_625*S27+ROcp37_926*C27;
-    ROcp37_128 = ROcp37_126*C28+ROcp37_427*S28;
-    ROcp37_228 = ROcp37_226*C28+ROcp37_527*S28;
-    ROcp37_328 = ROcp37_326*C28+ROcp37_627*S28;
-    ROcp37_428 = -ROcp37_126*S28+ROcp37_427*C28;
-    ROcp37_528 = -ROcp37_226*S28+ROcp37_527*C28;
-    ROcp37_628 = -ROcp37_326*S28+ROcp37_627*C28;
-    OMcp37_25 = qd[5]*C4;
-    OMcp37_35 = qd[5]*S4;
-    OMcp37_16 = qd[4]+qd[6]*S5;
-    OMcp37_26 = OMcp37_25+qd[6]*ROcp37_85;
-    OMcp37_36 = OMcp37_35+qd[6]*ROcp37_95;
-    RLcp37_17 = ROcp37_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp37_27 = ROcp37_26*dpt[1][4]+ROcp37_85*dpt[3][4];
-    RLcp37_37 = ROcp37_36*dpt[1][4]+ROcp37_95*dpt[3][4];
-    POcp37_17 = q[1]+RLcp37_17;
-    POcp37_27 = q[2]+RLcp37_27;
-    POcp37_37 = q[3]+RLcp37_37;
-    OMcp37_17 = OMcp37_16+qd[23]*ROcp37_16;
-    OMcp37_27 = OMcp37_26+qd[23]*ROcp37_26;
-    OMcp37_37 = OMcp37_36+qd[23]*ROcp37_36;
-    ORcp37_17 = OMcp37_26*RLcp37_37-OMcp37_36*RLcp37_27;
-    ORcp37_27 = -OMcp37_16*RLcp37_37+OMcp37_36*RLcp37_17;
-    ORcp37_37 = OMcp37_16*RLcp37_27-OMcp37_26*RLcp37_17;
-    VIcp37_17 = qd[1]+ORcp37_17;
-    VIcp37_27 = qd[2]+ORcp37_27;
-    VIcp37_37 = qd[3]+ORcp37_37;
-    OMcp37_18 = OMcp37_17+qd[24]*ROcp37_423;
-    OMcp37_28 = OMcp37_27+qd[24]*ROcp37_523;
-    OMcp37_38 = OMcp37_37+qd[24]*ROcp37_623;
-    RLcp37_19 = ROcp37_724*dpt[3][44];
-    RLcp37_29 = ROcp37_824*dpt[3][44];
-    RLcp37_39 = ROcp37_924*dpt[3][44];
-    POcp37_19 = POcp37_17+RLcp37_19;
-    POcp37_29 = POcp37_27+RLcp37_29;
-    POcp37_39 = POcp37_37+RLcp37_39;
-    OMcp37_19 = OMcp37_18+qd[25]*ROcp37_724;
-    OMcp37_29 = OMcp37_28+qd[25]*ROcp37_824;
-    OMcp37_39 = OMcp37_38+qd[25]*ROcp37_924;
-    ORcp37_19 = OMcp37_28*RLcp37_39-OMcp37_38*RLcp37_29;
-    ORcp37_29 = -OMcp37_18*RLcp37_39+OMcp37_38*RLcp37_19;
-    ORcp37_39 = OMcp37_18*RLcp37_29-OMcp37_28*RLcp37_19;
-    VIcp37_19 = ORcp37_19+VIcp37_17;
-    VIcp37_29 = ORcp37_29+VIcp37_27;
-    VIcp37_39 = ORcp37_39+VIcp37_37;
-    RLcp37_110 = ROcp37_125*dpt[1][48]+ROcp37_425*dpt[2][48]+ROcp37_724*dpt[3][48];
-    RLcp37_210 = ROcp37_225*dpt[1][48]+ROcp37_525*dpt[2][48]+ROcp37_824*dpt[3][48];
-    RLcp37_310 = ROcp37_325*dpt[1][48]+ROcp37_625*dpt[2][48]+ROcp37_924*dpt[3][48];
-    POcp37_110 = POcp37_19+RLcp37_110;
-    POcp37_210 = POcp37_29+RLcp37_210;
-    POcp37_310 = POcp37_39+RLcp37_310;
-    OMcp37_110 = OMcp37_19+qd[26]*ROcp37_425;
-    OMcp37_210 = OMcp37_29+qd[26]*ROcp37_525;
-    OMcp37_310 = OMcp37_39+qd[26]*ROcp37_625;
-    ORcp37_110 = OMcp37_29*RLcp37_310-OMcp37_39*RLcp37_210;
-    ORcp37_210 = -OMcp37_19*RLcp37_310+OMcp37_39*RLcp37_110;
-    ORcp37_310 = OMcp37_19*RLcp37_210-OMcp37_29*RLcp37_110;
-    VIcp37_110 = ORcp37_110+VIcp37_19;
-    VIcp37_210 = ORcp37_210+VIcp37_29;
-    VIcp37_310 = ORcp37_310+VIcp37_39;
-    RLcp37_111 = ROcp37_425*dpt[2][51];
-    RLcp37_211 = ROcp37_525*dpt[2][51];
-    RLcp37_311 = ROcp37_625*dpt[2][51];
-    POcp37_111 = POcp37_110+RLcp37_111;
-    POcp37_211 = POcp37_210+RLcp37_211;
-    POcp37_311 = POcp37_310+RLcp37_311;
-    OMcp37_111 = OMcp37_110+qd[27]*ROcp37_126;
-    OMcp37_211 = OMcp37_210+qd[27]*ROcp37_226;
-    OMcp37_311 = OMcp37_310+qd[27]*ROcp37_326;
-    ORcp37_111 = OMcp37_210*RLcp37_311-OMcp37_310*RLcp37_211;
-    ORcp37_211 = -OMcp37_110*RLcp37_311+OMcp37_310*RLcp37_111;
-    ORcp37_311 = OMcp37_110*RLcp37_211-OMcp37_210*RLcp37_111;
-    VIcp37_111 = ORcp37_111+VIcp37_110;
-    VIcp37_211 = ORcp37_211+VIcp37_210;
-    VIcp37_311 = ORcp37_311+VIcp37_310;
-    RLcp37_112 = ROcp37_727*dpt[3][53];
-    RLcp37_212 = ROcp37_827*dpt[3][53];
-    RLcp37_312 = ROcp37_927*dpt[3][53];
-    POcp37_112 = POcp37_111+RLcp37_112;
-    POcp37_212 = POcp37_211+RLcp37_212;
-    POcp37_312 = POcp37_311+RLcp37_312;
-    OMcp37_112 = OMcp37_111+qd[28]*ROcp37_727;
-    OMcp37_212 = OMcp37_211+qd[28]*ROcp37_827;
-    OMcp37_312 = OMcp37_311+qd[28]*ROcp37_927;
-    ORcp37_112 = OMcp37_211*RLcp37_312-OMcp37_311*RLcp37_212;
-    ORcp37_212 = -OMcp37_111*RLcp37_312+OMcp37_311*RLcp37_112;
-    ORcp37_312 = OMcp37_111*RLcp37_212-OMcp37_211*RLcp37_112;
-    VIcp37_112 = ORcp37_112+VIcp37_111;
-    VIcp37_212 = ORcp37_212+VIcp37_211;
-    VIcp37_312 = ORcp37_312+VIcp37_311;
-    PxF11[1] = POcp37_112;
-    PxF11[2] = POcp37_212;
-    PxF11[3] = POcp37_312;
-    RxF11[1][1] = ROcp37_128;
-    RxF11[1][2] = ROcp37_228;
-    RxF11[1][3] = ROcp37_328;
-    RxF11[2][1] = ROcp37_428;
-    RxF11[2][2] = ROcp37_528;
-    RxF11[2][3] = ROcp37_628;
-    RxF11[3][1] = ROcp37_727;
-    RxF11[3][2] = ROcp37_827;
-    RxF11[3][3] = ROcp37_927;
-    VxF11[1] = VIcp37_112;
-    VxF11[2] = VIcp37_212;
-    VxF11[3] = VIcp37_312;
-    OMxF11[1] = OMcp37_112;
-    OMxF11[2] = OMcp37_212;
-    OMxF11[3] = OMcp37_312;
-    ROcp38_25 = S4*S5;
-    ROcp38_35 = -C4*S5;
-    ROcp38_85 = -S4*C5;
-    ROcp38_95 = C4*C5;
-    ROcp38_16 = C5*C6;
-    ROcp38_26 = ROcp38_25*C6+C4*S6;
-    ROcp38_36 = ROcp38_35*C6+S4*S6;
-    ROcp38_46 = -C5*S6;
-    ROcp38_56 = -ROcp38_25*S6+C4*C6;
-    ROcp38_66 = -ROcp38_35*S6+S4*C6;
-    ROcp38_423 = ROcp38_46*C23+S23*S5;
-    ROcp38_523 = ROcp38_56*C23+ROcp38_85*S23;
-    ROcp38_623 = ROcp38_66*C23+ROcp38_95*S23;
-    ROcp38_723 = -ROcp38_46*S23+C23*S5;
-    ROcp38_823 = -ROcp38_56*S23+ROcp38_85*C23;
-    ROcp38_923 = -ROcp38_66*S23+ROcp38_95*C23;
-    ROcp38_124 = ROcp38_16*C24-ROcp38_723*S24;
-    ROcp38_224 = ROcp38_26*C24-ROcp38_823*S24;
-    ROcp38_324 = ROcp38_36*C24-ROcp38_923*S24;
-    ROcp38_724 = ROcp38_16*S24+ROcp38_723*C24;
-    ROcp38_824 = ROcp38_26*S24+ROcp38_823*C24;
-    ROcp38_924 = ROcp38_36*S24+ROcp38_923*C24;
-    ROcp38_125 = ROcp38_124*C25+ROcp38_423*S25;
-    ROcp38_225 = ROcp38_224*C25+ROcp38_523*S25;
-    ROcp38_325 = ROcp38_324*C25+ROcp38_623*S25;
-    ROcp38_425 = -ROcp38_124*S25+ROcp38_423*C25;
-    ROcp38_525 = -ROcp38_224*S25+ROcp38_523*C25;
-    ROcp38_625 = -ROcp38_324*S25+ROcp38_623*C25;
-    ROcp38_126 = ROcp38_125*C26-ROcp38_724*S26;
-    ROcp38_226 = ROcp38_225*C26-ROcp38_824*S26;
-    ROcp38_326 = ROcp38_325*C26-ROcp38_924*S26;
-    ROcp38_726 = ROcp38_125*S26+ROcp38_724*C26;
-    ROcp38_826 = ROcp38_225*S26+ROcp38_824*C26;
-    ROcp38_926 = ROcp38_325*S26+ROcp38_924*C26;
-    ROcp38_427 = ROcp38_425*C27+ROcp38_726*S27;
-    ROcp38_527 = ROcp38_525*C27+ROcp38_826*S27;
-    ROcp38_627 = ROcp38_625*C27+ROcp38_926*S27;
-    ROcp38_727 = -ROcp38_425*S27+ROcp38_726*C27;
-    ROcp38_827 = -ROcp38_525*S27+ROcp38_826*C27;
-    ROcp38_927 = -ROcp38_625*S27+ROcp38_926*C27;
-    ROcp38_128 = ROcp38_126*C28+ROcp38_427*S28;
-    ROcp38_228 = ROcp38_226*C28+ROcp38_527*S28;
-    ROcp38_328 = ROcp38_326*C28+ROcp38_627*S28;
-    ROcp38_428 = -ROcp38_126*S28+ROcp38_427*C28;
-    ROcp38_528 = -ROcp38_226*S28+ROcp38_527*C28;
-    ROcp38_628 = -ROcp38_326*S28+ROcp38_627*C28;
-    ROcp38_129 = ROcp38_128*C29-ROcp38_727*S29;
-    ROcp38_229 = ROcp38_228*C29-ROcp38_827*S29;
-    ROcp38_329 = ROcp38_328*C29-ROcp38_927*S29;
-    ROcp38_729 = ROcp38_128*S29+ROcp38_727*C29;
-    ROcp38_829 = ROcp38_228*S29+ROcp38_827*C29;
-    ROcp38_929 = ROcp38_328*S29+ROcp38_927*C29;
-    OMcp38_25 = qd[5]*C4;
-    OMcp38_35 = qd[5]*S4;
-    OMcp38_16 = qd[4]+qd[6]*S5;
-    OMcp38_26 = OMcp38_25+qd[6]*ROcp38_85;
-    OMcp38_36 = OMcp38_35+qd[6]*ROcp38_95;
-    RLcp38_17 = ROcp38_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp38_27 = ROcp38_26*dpt[1][4]+ROcp38_85*dpt[3][4];
-    RLcp38_37 = ROcp38_36*dpt[1][4]+ROcp38_95*dpt[3][4];
-    POcp38_17 = q[1]+RLcp38_17;
-    POcp38_27 = q[2]+RLcp38_27;
-    POcp38_37 = q[3]+RLcp38_37;
-    OMcp38_17 = OMcp38_16+qd[23]*ROcp38_16;
-    OMcp38_27 = OMcp38_26+qd[23]*ROcp38_26;
-    OMcp38_37 = OMcp38_36+qd[23]*ROcp38_36;
-    ORcp38_17 = OMcp38_26*RLcp38_37-OMcp38_36*RLcp38_27;
-    ORcp38_27 = -OMcp38_16*RLcp38_37+OMcp38_36*RLcp38_17;
-    ORcp38_37 = OMcp38_16*RLcp38_27-OMcp38_26*RLcp38_17;
-    VIcp38_17 = qd[1]+ORcp38_17;
-    VIcp38_27 = qd[2]+ORcp38_27;
-    VIcp38_37 = qd[3]+ORcp38_37;
-    OMcp38_18 = OMcp38_17+qd[24]*ROcp38_423;
-    OMcp38_28 = OMcp38_27+qd[24]*ROcp38_523;
-    OMcp38_38 = OMcp38_37+qd[24]*ROcp38_623;
-    RLcp38_19 = ROcp38_724*dpt[3][44];
-    RLcp38_29 = ROcp38_824*dpt[3][44];
-    RLcp38_39 = ROcp38_924*dpt[3][44];
-    POcp38_19 = POcp38_17+RLcp38_19;
-    POcp38_29 = POcp38_27+RLcp38_29;
-    POcp38_39 = POcp38_37+RLcp38_39;
-    OMcp38_19 = OMcp38_18+qd[25]*ROcp38_724;
-    OMcp38_29 = OMcp38_28+qd[25]*ROcp38_824;
-    OMcp38_39 = OMcp38_38+qd[25]*ROcp38_924;
-    ORcp38_19 = OMcp38_28*RLcp38_39-OMcp38_38*RLcp38_29;
-    ORcp38_29 = -OMcp38_18*RLcp38_39+OMcp38_38*RLcp38_19;
-    ORcp38_39 = OMcp38_18*RLcp38_29-OMcp38_28*RLcp38_19;
-    VIcp38_19 = ORcp38_19+VIcp38_17;
-    VIcp38_29 = ORcp38_29+VIcp38_27;
-    VIcp38_39 = ORcp38_39+VIcp38_37;
-    RLcp38_110 = ROcp38_125*dpt[1][48]+ROcp38_425*dpt[2][48]+ROcp38_724*dpt[3][48];
-    RLcp38_210 = ROcp38_225*dpt[1][48]+ROcp38_525*dpt[2][48]+ROcp38_824*dpt[3][48];
-    RLcp38_310 = ROcp38_325*dpt[1][48]+ROcp38_625*dpt[2][48]+ROcp38_924*dpt[3][48];
-    POcp38_110 = POcp38_19+RLcp38_110;
-    POcp38_210 = POcp38_29+RLcp38_210;
-    POcp38_310 = POcp38_39+RLcp38_310;
-    OMcp38_110 = OMcp38_19+qd[26]*ROcp38_425;
-    OMcp38_210 = OMcp38_29+qd[26]*ROcp38_525;
-    OMcp38_310 = OMcp38_39+qd[26]*ROcp38_625;
-    ORcp38_110 = OMcp38_29*RLcp38_310-OMcp38_39*RLcp38_210;
-    ORcp38_210 = -OMcp38_19*RLcp38_310+OMcp38_39*RLcp38_110;
-    ORcp38_310 = OMcp38_19*RLcp38_210-OMcp38_29*RLcp38_110;
-    VIcp38_110 = ORcp38_110+VIcp38_19;
-    VIcp38_210 = ORcp38_210+VIcp38_29;
-    VIcp38_310 = ORcp38_310+VIcp38_39;
-    RLcp38_111 = ROcp38_425*dpt[2][51];
-    RLcp38_211 = ROcp38_525*dpt[2][51];
-    RLcp38_311 = ROcp38_625*dpt[2][51];
-    POcp38_111 = POcp38_110+RLcp38_111;
-    POcp38_211 = POcp38_210+RLcp38_211;
-    POcp38_311 = POcp38_310+RLcp38_311;
-    OMcp38_111 = OMcp38_110+qd[27]*ROcp38_126;
-    OMcp38_211 = OMcp38_210+qd[27]*ROcp38_226;
-    OMcp38_311 = OMcp38_310+qd[27]*ROcp38_326;
-    ORcp38_111 = OMcp38_210*RLcp38_311-OMcp38_310*RLcp38_211;
-    ORcp38_211 = -OMcp38_110*RLcp38_311+OMcp38_310*RLcp38_111;
-    ORcp38_311 = OMcp38_110*RLcp38_211-OMcp38_210*RLcp38_111;
-    VIcp38_111 = ORcp38_111+VIcp38_110;
-    VIcp38_211 = ORcp38_211+VIcp38_210;
-    VIcp38_311 = ORcp38_311+VIcp38_310;
-    RLcp38_112 = ROcp38_727*dpt[3][53];
-    RLcp38_212 = ROcp38_827*dpt[3][53];
-    RLcp38_312 = ROcp38_927*dpt[3][53];
-    POcp38_112 = POcp38_111+RLcp38_112;
-    POcp38_212 = POcp38_211+RLcp38_212;
-    POcp38_312 = POcp38_311+RLcp38_312;
-    OMcp38_112 = OMcp38_111+qd[28]*ROcp38_727;
-    OMcp38_212 = OMcp38_211+qd[28]*ROcp38_827;
-    OMcp38_312 = OMcp38_311+qd[28]*ROcp38_927;
-    ORcp38_112 = OMcp38_211*RLcp38_312-OMcp38_311*RLcp38_212;
-    ORcp38_212 = -OMcp38_111*RLcp38_312+OMcp38_311*RLcp38_112;
-    ORcp38_312 = OMcp38_111*RLcp38_212-OMcp38_211*RLcp38_112;
-    VIcp38_112 = ORcp38_112+VIcp38_111;
-    VIcp38_212 = ORcp38_212+VIcp38_211;
-    VIcp38_312 = ORcp38_312+VIcp38_311;
-    RLcp38_113 = ROcp38_727*dpt[3][56];
-    RLcp38_213 = ROcp38_827*dpt[3][56];
-    RLcp38_313 = ROcp38_927*dpt[3][56];
-    POcp38_113 = POcp38_112+RLcp38_113;
-    POcp38_213 = POcp38_212+RLcp38_213;
-    POcp38_313 = POcp38_312+RLcp38_313;
-    OMcp38_113 = OMcp38_112+qd[29]*ROcp38_428;
-    OMcp38_213 = OMcp38_212+qd[29]*ROcp38_528;
-    OMcp38_313 = OMcp38_312+qd[29]*ROcp38_628;
-    ORcp38_113 = OMcp38_212*RLcp38_313-OMcp38_312*RLcp38_213;
-    ORcp38_213 = -OMcp38_112*RLcp38_313+OMcp38_312*RLcp38_113;
-    ORcp38_313 = OMcp38_112*RLcp38_213-OMcp38_212*RLcp38_113;
-    VIcp38_113 = ORcp38_113+VIcp38_112;
-    VIcp38_213 = ORcp38_213+VIcp38_212;
-    VIcp38_313 = ORcp38_313+VIcp38_312;
-    PxF12[1] = POcp38_113;
-    PxF12[2] = POcp38_213;
-    PxF12[3] = POcp38_313;
-    RxF12[1][1] = ROcp38_129;
-    RxF12[1][2] = ROcp38_229;
-    RxF12[1][3] = ROcp38_329;
-    RxF12[2][1] = ROcp38_428;
-    RxF12[2][2] = ROcp38_528;
-    RxF12[2][3] = ROcp38_628;
-    RxF12[3][1] = ROcp38_729;
-    RxF12[3][2] = ROcp38_829;
-    RxF12[3][3] = ROcp38_929;
-    VxF12[1] = VIcp38_113;
-    VxF12[2] = VIcp38_213;
-    VxF12[3] = VIcp38_313;
-    OMxF12[1] = OMcp38_113;
-    OMxF12[2] = OMcp38_213;
-    OMxF12[3] = OMcp38_313;
-    ROcp39_25 = S4*S5;
-    ROcp39_35 = -C4*S5;
-    ROcp39_85 = -S4*C5;
-    ROcp39_95 = C4*C5;
-    ROcp39_16 = C5*C6;
-    ROcp39_26 = ROcp39_25*C6+C4*S6;
-    ROcp39_36 = ROcp39_35*C6+S4*S6;
-    ROcp39_46 = -C5*S6;
-    ROcp39_56 = -ROcp39_25*S6+C4*C6;
-    ROcp39_66 = -ROcp39_35*S6+S4*C6;
-    ROcp39_423 = ROcp39_46*C23+S23*S5;
-    ROcp39_523 = ROcp39_56*C23+ROcp39_85*S23;
-    ROcp39_623 = ROcp39_66*C23+ROcp39_95*S23;
-    ROcp39_723 = -ROcp39_46*S23+C23*S5;
-    ROcp39_823 = -ROcp39_56*S23+ROcp39_85*C23;
-    ROcp39_923 = -ROcp39_66*S23+ROcp39_95*C23;
-    ROcp39_124 = ROcp39_16*C24-ROcp39_723*S24;
-    ROcp39_224 = ROcp39_26*C24-ROcp39_823*S24;
-    ROcp39_324 = ROcp39_36*C24-ROcp39_923*S24;
-    ROcp39_724 = ROcp39_16*S24+ROcp39_723*C24;
-    ROcp39_824 = ROcp39_26*S24+ROcp39_823*C24;
-    ROcp39_924 = ROcp39_36*S24+ROcp39_923*C24;
-    ROcp39_125 = ROcp39_124*C25+ROcp39_423*S25;
-    ROcp39_225 = ROcp39_224*C25+ROcp39_523*S25;
-    ROcp39_325 = ROcp39_324*C25+ROcp39_623*S25;
-    ROcp39_425 = -ROcp39_124*S25+ROcp39_423*C25;
-    ROcp39_525 = -ROcp39_224*S25+ROcp39_523*C25;
-    ROcp39_625 = -ROcp39_324*S25+ROcp39_623*C25;
-    ROcp39_130 = ROcp39_125*C30-ROcp39_724*S30;
-    ROcp39_230 = ROcp39_225*C30-ROcp39_824*S30;
-    ROcp39_330 = ROcp39_325*C30-ROcp39_924*S30;
-    ROcp39_730 = ROcp39_125*S30+ROcp39_724*C30;
-    ROcp39_830 = ROcp39_225*S30+ROcp39_824*C30;
-    ROcp39_930 = ROcp39_325*S30+ROcp39_924*C30;
-    ROcp39_431 = ROcp39_425*C31+ROcp39_730*S31;
-    ROcp39_531 = ROcp39_525*C31+ROcp39_830*S31;
-    ROcp39_631 = ROcp39_625*C31+ROcp39_930*S31;
-    ROcp39_731 = -ROcp39_425*S31+ROcp39_730*C31;
-    ROcp39_831 = -ROcp39_525*S31+ROcp39_830*C31;
-    ROcp39_931 = -ROcp39_625*S31+ROcp39_930*C31;
-    ROcp39_132 = ROcp39_130*C32+ROcp39_431*S32;
-    ROcp39_232 = ROcp39_230*C32+ROcp39_531*S32;
-    ROcp39_332 = ROcp39_330*C32+ROcp39_631*S32;
-    ROcp39_432 = -ROcp39_130*S32+ROcp39_431*C32;
-    ROcp39_532 = -ROcp39_230*S32+ROcp39_531*C32;
-    ROcp39_632 = -ROcp39_330*S32+ROcp39_631*C32;
-    OMcp39_25 = qd[5]*C4;
-    OMcp39_35 = qd[5]*S4;
-    OMcp39_16 = qd[4]+qd[6]*S5;
-    OMcp39_26 = OMcp39_25+qd[6]*ROcp39_85;
-    OMcp39_36 = OMcp39_35+qd[6]*ROcp39_95;
-    RLcp39_17 = ROcp39_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp39_27 = ROcp39_26*dpt[1][4]+ROcp39_85*dpt[3][4];
-    RLcp39_37 = ROcp39_36*dpt[1][4]+ROcp39_95*dpt[3][4];
-    POcp39_17 = q[1]+RLcp39_17;
-    POcp39_27 = q[2]+RLcp39_27;
-    POcp39_37 = q[3]+RLcp39_37;
-    OMcp39_17 = OMcp39_16+qd[23]*ROcp39_16;
-    OMcp39_27 = OMcp39_26+qd[23]*ROcp39_26;
-    OMcp39_37 = OMcp39_36+qd[23]*ROcp39_36;
-    ORcp39_17 = OMcp39_26*RLcp39_37-OMcp39_36*RLcp39_27;
-    ORcp39_27 = -OMcp39_16*RLcp39_37+OMcp39_36*RLcp39_17;
-    ORcp39_37 = OMcp39_16*RLcp39_27-OMcp39_26*RLcp39_17;
-    VIcp39_17 = qd[1]+ORcp39_17;
-    VIcp39_27 = qd[2]+ORcp39_27;
-    VIcp39_37 = qd[3]+ORcp39_37;
-    OMcp39_18 = OMcp39_17+qd[24]*ROcp39_423;
-    OMcp39_28 = OMcp39_27+qd[24]*ROcp39_523;
-    OMcp39_38 = OMcp39_37+qd[24]*ROcp39_623;
-    RLcp39_19 = ROcp39_724*dpt[3][44];
-    RLcp39_29 = ROcp39_824*dpt[3][44];
-    RLcp39_39 = ROcp39_924*dpt[3][44];
-    POcp39_19 = POcp39_17+RLcp39_19;
-    POcp39_29 = POcp39_27+RLcp39_29;
-    POcp39_39 = POcp39_37+RLcp39_39;
-    OMcp39_19 = OMcp39_18+qd[25]*ROcp39_724;
-    OMcp39_29 = OMcp39_28+qd[25]*ROcp39_824;
-    OMcp39_39 = OMcp39_38+qd[25]*ROcp39_924;
-    ORcp39_19 = OMcp39_28*RLcp39_39-OMcp39_38*RLcp39_29;
-    ORcp39_29 = -OMcp39_18*RLcp39_39+OMcp39_38*RLcp39_19;
-    ORcp39_39 = OMcp39_18*RLcp39_29-OMcp39_28*RLcp39_19;
-    VIcp39_19 = ORcp39_19+VIcp39_17;
-    VIcp39_29 = ORcp39_29+VIcp39_27;
-    VIcp39_39 = ORcp39_39+VIcp39_37;
-    RLcp39_110 = ROcp39_125*dpt[1][49]+ROcp39_425*dpt[2][49]+ROcp39_724*dpt[3][49];
-    RLcp39_210 = ROcp39_225*dpt[1][49]+ROcp39_525*dpt[2][49]+ROcp39_824*dpt[3][49];
-    RLcp39_310 = ROcp39_325*dpt[1][49]+ROcp39_625*dpt[2][49]+ROcp39_924*dpt[3][49];
-    POcp39_110 = POcp39_19+RLcp39_110;
-    POcp39_210 = POcp39_29+RLcp39_210;
-    POcp39_310 = POcp39_39+RLcp39_310;
-    OMcp39_110 = OMcp39_19+qd[30]*ROcp39_425;
-    OMcp39_210 = OMcp39_29+qd[30]*ROcp39_525;
-    OMcp39_310 = OMcp39_39+qd[30]*ROcp39_625;
-    ORcp39_110 = OMcp39_29*RLcp39_310-OMcp39_39*RLcp39_210;
-    ORcp39_210 = -OMcp39_19*RLcp39_310+OMcp39_39*RLcp39_110;
-    ORcp39_310 = OMcp39_19*RLcp39_210-OMcp39_29*RLcp39_110;
-    VIcp39_110 = ORcp39_110+VIcp39_19;
-    VIcp39_210 = ORcp39_210+VIcp39_29;
-    VIcp39_310 = ORcp39_310+VIcp39_39;
-    RLcp39_111 = ROcp39_425*dpt[2][60];
-    RLcp39_211 = ROcp39_525*dpt[2][60];
-    RLcp39_311 = ROcp39_625*dpt[2][60];
-    POcp39_111 = POcp39_110+RLcp39_111;
-    POcp39_211 = POcp39_210+RLcp39_211;
-    POcp39_311 = POcp39_310+RLcp39_311;
-    OMcp39_111 = OMcp39_110+qd[31]*ROcp39_130;
-    OMcp39_211 = OMcp39_210+qd[31]*ROcp39_230;
-    OMcp39_311 = OMcp39_310+qd[31]*ROcp39_330;
-    ORcp39_111 = OMcp39_210*RLcp39_311-OMcp39_310*RLcp39_211;
-    ORcp39_211 = -OMcp39_110*RLcp39_311+OMcp39_310*RLcp39_111;
-    ORcp39_311 = OMcp39_110*RLcp39_211-OMcp39_210*RLcp39_111;
-    VIcp39_111 = ORcp39_111+VIcp39_110;
-    VIcp39_211 = ORcp39_211+VIcp39_210;
-    VIcp39_311 = ORcp39_311+VIcp39_310;
-    RLcp39_112 = ROcp39_731*dpt[3][62];
-    RLcp39_212 = ROcp39_831*dpt[3][62];
-    RLcp39_312 = ROcp39_931*dpt[3][62];
-    POcp39_112 = POcp39_111+RLcp39_112;
-    POcp39_212 = POcp39_211+RLcp39_212;
-    POcp39_312 = POcp39_311+RLcp39_312;
-    OMcp39_112 = OMcp39_111+qd[32]*ROcp39_731;
-    OMcp39_212 = OMcp39_211+qd[32]*ROcp39_831;
-    OMcp39_312 = OMcp39_311+qd[32]*ROcp39_931;
-    ORcp39_112 = OMcp39_211*RLcp39_312-OMcp39_311*RLcp39_212;
-    ORcp39_212 = -OMcp39_111*RLcp39_312+OMcp39_311*RLcp39_112;
-    ORcp39_312 = OMcp39_111*RLcp39_212-OMcp39_211*RLcp39_112;
-    VIcp39_112 = ORcp39_112+VIcp39_111;
-    VIcp39_212 = ORcp39_212+VIcp39_211;
-    VIcp39_312 = ORcp39_312+VIcp39_311;
-    PxF13[1] = POcp39_112;
-    PxF13[2] = POcp39_212;
-    PxF13[3] = POcp39_312;
-    RxF13[1][1] = ROcp39_132;
-    RxF13[1][2] = ROcp39_232;
-    RxF13[1][3] = ROcp39_332;
-    RxF13[2][1] = ROcp39_432;
-    RxF13[2][2] = ROcp39_532;
-    RxF13[2][3] = ROcp39_632;
-    RxF13[3][1] = ROcp39_731;
-    RxF13[3][2] = ROcp39_831;
-    RxF13[3][3] = ROcp39_931;
-    VxF13[1] = VIcp39_112;
-    VxF13[2] = VIcp39_212;
-    VxF13[3] = VIcp39_312;
-    OMxF13[1] = OMcp39_112;
-    OMxF13[2] = OMcp39_212;
-    OMxF13[3] = OMcp39_312;
-    ROcp40_25 = S4*S5;
-    ROcp40_35 = -C4*S5;
-    ROcp40_85 = -S4*C5;
-    ROcp40_95 = C4*C5;
-    ROcp40_16 = C5*C6;
-    ROcp40_26 = ROcp40_25*C6+C4*S6;
-    ROcp40_36 = ROcp40_35*C6+S4*S6;
-    ROcp40_46 = -C5*S6;
-    ROcp40_56 = -ROcp40_25*S6+C4*C6;
-    ROcp40_66 = -ROcp40_35*S6+S4*C6;
-    ROcp40_423 = ROcp40_46*C23+S23*S5;
-    ROcp40_523 = ROcp40_56*C23+ROcp40_85*S23;
-    ROcp40_623 = ROcp40_66*C23+ROcp40_95*S23;
-    ROcp40_723 = -ROcp40_46*S23+C23*S5;
-    ROcp40_823 = -ROcp40_56*S23+ROcp40_85*C23;
-    ROcp40_923 = -ROcp40_66*S23+ROcp40_95*C23;
-    ROcp40_124 = ROcp40_16*C24-ROcp40_723*S24;
-    ROcp40_224 = ROcp40_26*C24-ROcp40_823*S24;
-    ROcp40_324 = ROcp40_36*C24-ROcp40_923*S24;
-    ROcp40_724 = ROcp40_16*S24+ROcp40_723*C24;
-    ROcp40_824 = ROcp40_26*S24+ROcp40_823*C24;
-    ROcp40_924 = ROcp40_36*S24+ROcp40_923*C24;
-    ROcp40_125 = ROcp40_124*C25+ROcp40_423*S25;
-    ROcp40_225 = ROcp40_224*C25+ROcp40_523*S25;
-    ROcp40_325 = ROcp40_324*C25+ROcp40_623*S25;
-    ROcp40_425 = -ROcp40_124*S25+ROcp40_423*C25;
-    ROcp40_525 = -ROcp40_224*S25+ROcp40_523*C25;
-    ROcp40_625 = -ROcp40_324*S25+ROcp40_623*C25;
-    ROcp40_130 = ROcp40_125*C30-ROcp40_724*S30;
-    ROcp40_230 = ROcp40_225*C30-ROcp40_824*S30;
-    ROcp40_330 = ROcp40_325*C30-ROcp40_924*S30;
-    ROcp40_730 = ROcp40_125*S30+ROcp40_724*C30;
-    ROcp40_830 = ROcp40_225*S30+ROcp40_824*C30;
-    ROcp40_930 = ROcp40_325*S30+ROcp40_924*C30;
-    ROcp40_431 = ROcp40_425*C31+ROcp40_730*S31;
-    ROcp40_531 = ROcp40_525*C31+ROcp40_830*S31;
-    ROcp40_631 = ROcp40_625*C31+ROcp40_930*S31;
-    ROcp40_731 = -ROcp40_425*S31+ROcp40_730*C31;
-    ROcp40_831 = -ROcp40_525*S31+ROcp40_830*C31;
-    ROcp40_931 = -ROcp40_625*S31+ROcp40_930*C31;
-    ROcp40_132 = ROcp40_130*C32+ROcp40_431*S32;
-    ROcp40_232 = ROcp40_230*C32+ROcp40_531*S32;
-    ROcp40_332 = ROcp40_330*C32+ROcp40_631*S32;
-    ROcp40_432 = -ROcp40_130*S32+ROcp40_431*C32;
-    ROcp40_532 = -ROcp40_230*S32+ROcp40_531*C32;
-    ROcp40_632 = -ROcp40_330*S32+ROcp40_631*C32;
-    ROcp40_133 = ROcp40_132*C33-ROcp40_731*S33;
-    ROcp40_233 = ROcp40_232*C33-ROcp40_831*S33;
-    ROcp40_333 = ROcp40_332*C33-ROcp40_931*S33;
-    ROcp40_733 = ROcp40_132*S33+ROcp40_731*C33;
-    ROcp40_833 = ROcp40_232*S33+ROcp40_831*C33;
-    ROcp40_933 = ROcp40_332*S33+ROcp40_931*C33;
-    OMcp40_25 = qd[5]*C4;
-    OMcp40_35 = qd[5]*S4;
-    OMcp40_16 = qd[4]+qd[6]*S5;
-    OMcp40_26 = OMcp40_25+qd[6]*ROcp40_85;
-    OMcp40_36 = OMcp40_35+qd[6]*ROcp40_95;
-    RLcp40_17 = ROcp40_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp40_27 = ROcp40_26*dpt[1][4]+ROcp40_85*dpt[3][4];
-    RLcp40_37 = ROcp40_36*dpt[1][4]+ROcp40_95*dpt[3][4];
-    POcp40_17 = q[1]+RLcp40_17;
-    POcp40_27 = q[2]+RLcp40_27;
-    POcp40_37 = q[3]+RLcp40_37;
-    OMcp40_17 = OMcp40_16+qd[23]*ROcp40_16;
-    OMcp40_27 = OMcp40_26+qd[23]*ROcp40_26;
-    OMcp40_37 = OMcp40_36+qd[23]*ROcp40_36;
-    ORcp40_17 = OMcp40_26*RLcp40_37-OMcp40_36*RLcp40_27;
-    ORcp40_27 = -OMcp40_16*RLcp40_37+OMcp40_36*RLcp40_17;
-    ORcp40_37 = OMcp40_16*RLcp40_27-OMcp40_26*RLcp40_17;
-    VIcp40_17 = qd[1]+ORcp40_17;
-    VIcp40_27 = qd[2]+ORcp40_27;
-    VIcp40_37 = qd[3]+ORcp40_37;
-    OMcp40_18 = OMcp40_17+qd[24]*ROcp40_423;
-    OMcp40_28 = OMcp40_27+qd[24]*ROcp40_523;
-    OMcp40_38 = OMcp40_37+qd[24]*ROcp40_623;
-    RLcp40_19 = ROcp40_724*dpt[3][44];
-    RLcp40_29 = ROcp40_824*dpt[3][44];
-    RLcp40_39 = ROcp40_924*dpt[3][44];
-    POcp40_19 = POcp40_17+RLcp40_19;
-    POcp40_29 = POcp40_27+RLcp40_29;
-    POcp40_39 = POcp40_37+RLcp40_39;
-    OMcp40_19 = OMcp40_18+qd[25]*ROcp40_724;
-    OMcp40_29 = OMcp40_28+qd[25]*ROcp40_824;
-    OMcp40_39 = OMcp40_38+qd[25]*ROcp40_924;
-    ORcp40_19 = OMcp40_28*RLcp40_39-OMcp40_38*RLcp40_29;
-    ORcp40_29 = -OMcp40_18*RLcp40_39+OMcp40_38*RLcp40_19;
-    ORcp40_39 = OMcp40_18*RLcp40_29-OMcp40_28*RLcp40_19;
-    VIcp40_19 = ORcp40_19+VIcp40_17;
-    VIcp40_29 = ORcp40_29+VIcp40_27;
-    VIcp40_39 = ORcp40_39+VIcp40_37;
-    RLcp40_110 = ROcp40_125*dpt[1][49]+ROcp40_425*dpt[2][49]+ROcp40_724*dpt[3][49];
-    RLcp40_210 = ROcp40_225*dpt[1][49]+ROcp40_525*dpt[2][49]+ROcp40_824*dpt[3][49];
-    RLcp40_310 = ROcp40_325*dpt[1][49]+ROcp40_625*dpt[2][49]+ROcp40_924*dpt[3][49];
-    POcp40_110 = POcp40_19+RLcp40_110;
-    POcp40_210 = POcp40_29+RLcp40_210;
-    POcp40_310 = POcp40_39+RLcp40_310;
-    OMcp40_110 = OMcp40_19+qd[30]*ROcp40_425;
-    OMcp40_210 = OMcp40_29+qd[30]*ROcp40_525;
-    OMcp40_310 = OMcp40_39+qd[30]*ROcp40_625;
-    ORcp40_110 = OMcp40_29*RLcp40_310-OMcp40_39*RLcp40_210;
-    ORcp40_210 = -OMcp40_19*RLcp40_310+OMcp40_39*RLcp40_110;
-    ORcp40_310 = OMcp40_19*RLcp40_210-OMcp40_29*RLcp40_110;
-    VIcp40_110 = ORcp40_110+VIcp40_19;
-    VIcp40_210 = ORcp40_210+VIcp40_29;
-    VIcp40_310 = ORcp40_310+VIcp40_39;
-    RLcp40_111 = ROcp40_425*dpt[2][60];
-    RLcp40_211 = ROcp40_525*dpt[2][60];
-    RLcp40_311 = ROcp40_625*dpt[2][60];
-    POcp40_111 = POcp40_110+RLcp40_111;
-    POcp40_211 = POcp40_210+RLcp40_211;
-    POcp40_311 = POcp40_310+RLcp40_311;
-    OMcp40_111 = OMcp40_110+qd[31]*ROcp40_130;
-    OMcp40_211 = OMcp40_210+qd[31]*ROcp40_230;
-    OMcp40_311 = OMcp40_310+qd[31]*ROcp40_330;
-    ORcp40_111 = OMcp40_210*RLcp40_311-OMcp40_310*RLcp40_211;
-    ORcp40_211 = -OMcp40_110*RLcp40_311+OMcp40_310*RLcp40_111;
-    ORcp40_311 = OMcp40_110*RLcp40_211-OMcp40_210*RLcp40_111;
-    VIcp40_111 = ORcp40_111+VIcp40_110;
-    VIcp40_211 = ORcp40_211+VIcp40_210;
-    VIcp40_311 = ORcp40_311+VIcp40_310;
-    RLcp40_112 = ROcp40_731*dpt[3][62];
-    RLcp40_212 = ROcp40_831*dpt[3][62];
-    RLcp40_312 = ROcp40_931*dpt[3][62];
-    POcp40_112 = POcp40_111+RLcp40_112;
-    POcp40_212 = POcp40_211+RLcp40_212;
-    POcp40_312 = POcp40_311+RLcp40_312;
-    OMcp40_112 = OMcp40_111+qd[32]*ROcp40_731;
-    OMcp40_212 = OMcp40_211+qd[32]*ROcp40_831;
-    OMcp40_312 = OMcp40_311+qd[32]*ROcp40_931;
-    ORcp40_112 = OMcp40_211*RLcp40_312-OMcp40_311*RLcp40_212;
-    ORcp40_212 = -OMcp40_111*RLcp40_312+OMcp40_311*RLcp40_112;
-    ORcp40_312 = OMcp40_111*RLcp40_212-OMcp40_211*RLcp40_112;
-    VIcp40_112 = ORcp40_112+VIcp40_111;
-    VIcp40_212 = ORcp40_212+VIcp40_211;
-    VIcp40_312 = ORcp40_312+VIcp40_311;
-    RLcp40_113 = ROcp40_731*dpt[3][65];
-    RLcp40_213 = ROcp40_831*dpt[3][65];
-    RLcp40_313 = ROcp40_931*dpt[3][65];
-    POcp40_113 = POcp40_112+RLcp40_113;
-    POcp40_213 = POcp40_212+RLcp40_213;
-    POcp40_313 = POcp40_312+RLcp40_313;
-    OMcp40_113 = OMcp40_112+qd[33]*ROcp40_432;
-    OMcp40_213 = OMcp40_212+qd[33]*ROcp40_532;
-    OMcp40_313 = OMcp40_312+qd[33]*ROcp40_632;
-    ORcp40_113 = OMcp40_212*RLcp40_313-OMcp40_312*RLcp40_213;
-    ORcp40_213 = -OMcp40_112*RLcp40_313+OMcp40_312*RLcp40_113;
-    ORcp40_313 = OMcp40_112*RLcp40_213-OMcp40_212*RLcp40_113;
-    VIcp40_113 = ORcp40_113+VIcp40_112;
-    VIcp40_213 = ORcp40_213+VIcp40_212;
-    VIcp40_313 = ORcp40_313+VIcp40_312;
-    PxF14[1] = POcp40_113;
-    PxF14[2] = POcp40_213;
-    PxF14[3] = POcp40_313;
-    RxF14[1][1] = ROcp40_133;
-    RxF14[1][2] = ROcp40_233;
-    RxF14[1][3] = ROcp40_333;
-    RxF14[2][1] = ROcp40_432;
-    RxF14[2][2] = ROcp40_532;
-    RxF14[2][3] = ROcp40_632;
-    RxF14[3][1] = ROcp40_733;
-    RxF14[3][2] = ROcp40_833;
-    RxF14[3][3] = ROcp40_933;
-    VxF14[1] = VIcp40_113;
-    VxF14[2] = VIcp40_213;
-    VxF14[3] = VIcp40_313;
-    OMxF14[1] = OMcp40_113;
-    OMxF14[2] = OMcp40_213;
-    OMxF14[3] = OMcp40_313;
-    // output copy
-    for(i=1; i<=3; i++)
-    {
-        PxF_tab[0][i] = PxF1[i];
-        PxF_tab[1][i] = PxF2[i];
-        PxF_tab[2][i] = PxF3[i];
-        PxF_tab[3][i] = PxF4[i];
-        PxF_tab[4][i] = PxF5[i];
-        PxF_tab[5][i] = PxF6[i];
-        PxF_tab[6][i] = PxF7[i];
-        PxF_tab[7][i] = PxF8[i];
-        PxF_tab[8][i] = PxF9[i];
-        PxF_tab[9][i] = PxF10[i];
-        PxF_tab[10][i] = PxF11[i];
-        PxF_tab[11][i] = PxF12[i];
-        PxF_tab[12][i] = PxF13[i];
-        PxF_tab[13][i] = PxF14[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        for(j=1; j<=3; j++)
-        {
-        RxF_tab[0][i][j] = RxF1[i][j];
-        RxF_tab[1][i][j] = RxF2[i][j];
-        RxF_tab[2][i][j] = RxF3[i][j];
-        RxF_tab[3][i][j] = RxF4[i][j];
-        RxF_tab[4][i][j] = RxF5[i][j];
-        RxF_tab[5][i][j] = RxF6[i][j];
-        RxF_tab[6][i][j] = RxF7[i][j];
-        RxF_tab[7][i][j] = RxF8[i][j];
-        RxF_tab[8][i][j] = RxF9[i][j];
-        RxF_tab[9][i][j] = RxF10[i][j];
-        RxF_tab[10][i][j] = RxF11[i][j];
-        RxF_tab[11][i][j] = RxF12[i][j];
-        RxF_tab[12][i][j] = RxF13[i][j];
-        RxF_tab[13][i][j] = RxF14[i][j];
-        }
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        VxF_tab[0][i] = VxF1[i];
-        VxF_tab[1][i] = VxF2[i];
-        VxF_tab[2][i] = VxF3[i];
-        VxF_tab[3][i] = VxF4[i];
-        VxF_tab[4][i] = VxF5[i];
-        VxF_tab[5][i] = VxF6[i];
-        VxF_tab[6][i] = VxF7[i];
-        VxF_tab[7][i] = VxF8[i];
-        VxF_tab[8][i] = VxF9[i];
-        VxF_tab[9][i] = VxF10[i];
-        VxF_tab[10][i] = VxF11[i];
-        VxF_tab[11][i] = VxF12[i];
-        VxF_tab[12][i] = VxF13[i];
-        VxF_tab[13][i] = VxF14[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        OMxF_tab[0][i] = OMxF1[i];
-        OMxF_tab[1][i] = OMxF2[i];
-        OMxF_tab[2][i] = OMxF3[i];
-        OMxF_tab[3][i] = OMxF4[i];
-        OMxF_tab[4][i] = OMxF5[i];
-        OMxF_tab[5][i] = OMxF6[i];
-        OMxF_tab[6][i] = OMxF7[i];
-        OMxF_tab[7][i] = OMxF8[i];
-        OMxF_tab[8][i] = OMxF9[i];
-        OMxF_tab[9][i] = OMxF10[i];
-        OMxF_tab[10][i] = OMxF11[i];
-        OMxF_tab[11][i] = OMxF12[i];
-        OMxF_tab[12][i] = OMxF13[i];
-        OMxF_tab[13][i] = OMxF14[i];
-    }
-
-}
diff --git a/symbolicR/init_feet/CMakeLists.txt b/symbolicR/init_feet/CMakeLists.txt
index 3bae37b..c963c74 100644
--- a/symbolicR/init_feet/CMakeLists.txt
+++ b/symbolicR/init_feet/CMakeLists.txt
@@ -1,12 +1,22 @@
-# Copyright: (C)
-# Authors: Timothee Habra & Nicolas Van der Noot
-# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# CMake for compiling the symbolic files of a robotran project in C
+# 
 
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 #                       LIBRARY MAIN CONFIGURATIONS
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
-cmake_minimum_required(VERSION 2.8.7)
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
 
 project(Project_symbolic)
 
@@ -49,10 +59,26 @@ include_directories ( ${INCLUDE_DIR} )
 include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
 
 if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
+
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
     add_library(Project_symbolic SHARED ${SOURCE_FILES})
+	
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_symbolic PROPERTIES SUFFIX .so)    
+    endif()    
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
+        add_custom_command(TARGET Project_symbolic POST_BUILD
+			COMMAND ${CMAKE_COMMAND} -E copy
+                        ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_symbolic.dll
+                        ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_symbolic.dll
+        )
+    endif()
+
 else()
     add_library(Project_symbolic STATIC ${SOURCE_FILES})
-    add_definitions( -DPROJECT_USERFCT_STATIC_DEFINE)
 
     # some symbolic routines call user functions (ext_forces, accelred, ...)
     target_link_libraries(Project_symbolic Project_userfct)
@@ -67,14 +93,5 @@ endif()
 if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
     find_package( LibRobotranC REQUIRED )
     add_definitions(${LIB_MBSYSC_DEFINITIONS})
+    target_link_libraries(Project_symbolic ${LIB_MBSYSC_MODULES})
 endif()
-
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         STUFF FOR WINDOWS (TO BE CLEANED)
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-include (GenerateExportHeader)
-GENERATE_EXPORT_HEADER( Project_symbolic    )
-
-install (TARGETS Project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
diff --git a/symbolicR/init_feet/user_kinematics_model.c b/symbolicR/init_feet/user_kinematics_model.c
deleted file mode 100644
index 542f5ca..0000000
--- a/symbolicR/init_feet/user_kinematics_model.c
+++ /dev/null
@@ -1,1734 +0,0 @@
-#include "mbs_data.h"
-
-#include "math.h"
-
-/*! \brief compute the kinematics model of sensors with ExtForces
- * 
- * \param[in] mbs_data Robotran structure
- * \param[out] PxF_tab tabular with all the 'PxF' computed
- * \param[out] RxF_tab tabular with all the 'RxF' computed
- * \param[out] VxF_tab tabular with all the 'VxF' computed
- * \param[out] OMxF_tab tabular with all the 'OMxF' computed
- */
-void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
-{
-    // variables declaration
-    int i, j;
-    double *q, *qd;
-    double **dpt;
-
-    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
-    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
-    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
-    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
-
-    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
-    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
-
-    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
-    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
-    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
-    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
-    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
-    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
-    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
-    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
-    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
-    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
-    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
-    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
-    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
-    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
-    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
-    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
-    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
-    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
-    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
-    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
-    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
-    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
-    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
-    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
-    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
-    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
-    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
-    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
-    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
-    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
-    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
-    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
-    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
-    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
-    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
-    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
-    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
-    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
-    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
-    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
-    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
-    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
-    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
-    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
-    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
-    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
-    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
-    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
-    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
-    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
-    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
-    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
-    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
-    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
-    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
-    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
-    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
-    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
-    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
-    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
-    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
-    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
-    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
-    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
-    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
-    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
-    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
-    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
-    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
-    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
-    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
-    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
-    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
-    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
-    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
-    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
-    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
-    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
-    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
-    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
-    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
-    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
-    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
-    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
-    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
-
-    // variables initialization
-    q   = mbs_data->q;
-    qd  = mbs_data->qd;
-    dpt = mbs_data->dpt;
-
-    // cosines and sines
-    C4 = cos(q[4]);
-    C5 = cos(q[5]);
-    C6 = cos(q[6]);
-    C7 = cos(q[7]);
-    C8 = cos(q[8]);
-    C9 = cos(q[9]);
-    C10 = cos(q[10]);
-    C11 = cos(q[11]);
-    C12 = cos(q[12]);
-    C13 = cos(q[13]);
-    C14 = cos(q[14]);
-    C15 = cos(q[15]);
-    C16 = cos(q[16]);
-    C17 = cos(q[17]);
-    C18 = cos(q[18]);
-    C19 = cos(q[19]);
-    C20 = cos(q[20]);
-    C21 = cos(q[21]);
-    C22 = cos(q[22]);
-    C23 = cos(q[23]);
-    C24 = cos(q[24]);
-    C25 = cos(q[25]);
-    C26 = cos(q[26]);
-    C27 = cos(q[27]);
-    C28 = cos(q[28]);
-    C29 = cos(q[29]);
-    S4 = sin(q[4]);
-    S5 = sin(q[5]);
-    S6 = sin(q[6]);
-    S7 = sin(q[7]);
-    S8 = sin(q[8]);
-    S9 = sin(q[9]);
-    S10 = sin(q[10]);
-    S11 = sin(q[11]);
-    S12 = sin(q[12]);
-    S13 = sin(q[13]);
-    S14 = sin(q[14]);
-    S15 = sin(q[15]);
-    S16 = sin(q[16]);
-    S17 = sin(q[17]);
-    S18 = sin(q[18]);
-    S19 = sin(q[19]);
-    S20 = sin(q[20]);
-    S21 = sin(q[21]);
-    S22 = sin(q[22]);
-    S23 = sin(q[23]);
-    S24 = sin(q[24]);
-    S25 = sin(q[25]);
-    S26 = sin(q[26]);
-    S27 = sin(q[27]);
-    S28 = sin(q[28]);
-    S29 = sin(q[29]);
-
-    // main computation
-    ROcp25_25 = S4*S5;
-    ROcp25_35 = -C4*S5;
-    ROcp25_85 = -S4*C5;
-    ROcp25_95 = C4*C5;
-    ROcp25_16 = C5*C6;
-    ROcp25_26 = ROcp25_25*C6+C4*S6;
-    ROcp25_36 = ROcp25_35*C6+S4*S6;
-    ROcp25_46 = -C5*S6;
-    ROcp25_56 = -ROcp25_25*S6+C4*C6;
-    ROcp25_66 = -ROcp25_35*S6+S4*C6;
-    OMcp25_25 = qd[5]*C4;
-    OMcp25_35 = qd[5]*S4;
-    OMcp25_16 = qd[4]+qd[6]*S5;
-    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
-    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
-    PxF1[1] = q[1];
-    PxF1[2] = q[2];
-    PxF1[3] = q[3];
-    RxF1[1][1] = ROcp25_16;
-    RxF1[1][2] = ROcp25_26;
-    RxF1[1][3] = ROcp25_36;
-    RxF1[2][1] = ROcp25_46;
-    RxF1[2][2] = ROcp25_56;
-    RxF1[2][3] = ROcp25_66;
-    RxF1[3][1] = S5;
-    RxF1[3][2] = ROcp25_85;
-    RxF1[3][3] = ROcp25_95;
-    VxF1[1] = qd[1];
-    VxF1[2] = qd[2];
-    VxF1[3] = qd[3];
-    OMxF1[1] = OMcp25_16;
-    OMxF1[2] = OMcp25_26;
-    OMxF1[3] = OMcp25_36;
-    ROcp26_25 = S4*S5;
-    ROcp26_35 = -C4*S5;
-    ROcp26_85 = -S4*C5;
-    ROcp26_95 = C4*C5;
-    ROcp26_16 = C5*C6;
-    ROcp26_26 = ROcp26_25*C6+C4*S6;
-    ROcp26_36 = ROcp26_35*C6+S4*S6;
-    ROcp26_46 = -C5*S6;
-    ROcp26_56 = -ROcp26_25*S6+C4*C6;
-    ROcp26_66 = -ROcp26_35*S6+S4*C6;
-    ROcp26_17 = ROcp26_16*C7-S5*S7;
-    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
-    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
-    ROcp26_77 = ROcp26_16*S7+S5*C7;
-    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
-    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
-    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
-    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
-    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
-    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
-    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
-    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
-    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
-    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
-    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
-    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
-    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
-    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
-    OMcp26_25 = qd[5]*C4;
-    OMcp26_35 = qd[5]*S4;
-    OMcp26_16 = qd[4]+qd[6]*S5;
-    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
-    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
-    RLcp26_17 = ROcp26_46*dpt[2][2];
-    RLcp26_27 = ROcp26_56*dpt[2][2];
-    RLcp26_37 = ROcp26_66*dpt[2][2];
-    POcp26_17 = q[1]+RLcp26_17;
-    POcp26_27 = q[2]+RLcp26_27;
-    POcp26_37 = q[3]+RLcp26_37;
-    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
-    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
-    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
-    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
-    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
-    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
-    VIcp26_17 = qd[1]+ORcp26_17;
-    VIcp26_27 = qd[2]+ORcp26_27;
-    VIcp26_37 = qd[3]+ORcp26_37;
-    RLcp26_18 = ROcp26_46*dpt[2][6];
-    RLcp26_28 = ROcp26_56*dpt[2][6];
-    RLcp26_38 = ROcp26_66*dpt[2][6];
-    POcp26_18 = POcp26_17+RLcp26_18;
-    POcp26_28 = POcp26_27+RLcp26_28;
-    POcp26_38 = POcp26_37+RLcp26_38;
-    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
-    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
-    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
-    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
-    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
-    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
-    VIcp26_18 = ORcp26_18+VIcp26_17;
-    VIcp26_28 = ORcp26_28+VIcp26_27;
-    VIcp26_38 = ORcp26_38+VIcp26_37;
-    RLcp26_19 = ROcp26_78*dpt[3][8];
-    RLcp26_29 = ROcp26_88*dpt[3][8];
-    RLcp26_39 = ROcp26_98*dpt[3][8];
-    POcp26_19 = POcp26_18+RLcp26_19;
-    POcp26_29 = POcp26_28+RLcp26_29;
-    POcp26_39 = POcp26_38+RLcp26_39;
-    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
-    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
-    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
-    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
-    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
-    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
-    VIcp26_19 = ORcp26_19+VIcp26_18;
-    VIcp26_29 = ORcp26_29+VIcp26_28;
-    VIcp26_39 = ORcp26_39+VIcp26_38;
-    PxF2[1] = POcp26_19;
-    PxF2[2] = POcp26_29;
-    PxF2[3] = POcp26_39;
-    RxF2[1][1] = ROcp26_19;
-    RxF2[1][2] = ROcp26_29;
-    RxF2[1][3] = ROcp26_39;
-    RxF2[2][1] = ROcp26_49;
-    RxF2[2][2] = ROcp26_59;
-    RxF2[2][3] = ROcp26_69;
-    RxF2[3][1] = ROcp26_78;
-    RxF2[3][2] = ROcp26_88;
-    RxF2[3][3] = ROcp26_98;
-    VxF2[1] = VIcp26_19;
-    VxF2[2] = VIcp26_29;
-    VxF2[3] = VIcp26_39;
-    OMxF2[1] = OMcp26_19;
-    OMxF2[2] = OMcp26_29;
-    OMxF2[3] = OMcp26_39;
-    ROcp27_25 = S4*S5;
-    ROcp27_35 = -C4*S5;
-    ROcp27_85 = -S4*C5;
-    ROcp27_95 = C4*C5;
-    ROcp27_16 = C5*C6;
-    ROcp27_26 = ROcp27_25*C6+C4*S6;
-    ROcp27_36 = ROcp27_35*C6+S4*S6;
-    ROcp27_46 = -C5*S6;
-    ROcp27_56 = -ROcp27_25*S6+C4*C6;
-    ROcp27_66 = -ROcp27_35*S6+S4*C6;
-    ROcp27_17 = ROcp27_16*C7-S5*S7;
-    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
-    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
-    ROcp27_77 = ROcp27_16*S7+S5*C7;
-    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
-    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
-    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
-    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
-    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
-    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
-    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
-    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
-    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
-    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
-    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
-    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
-    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
-    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
-    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
-    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
-    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
-    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
-    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
-    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
-    OMcp27_25 = qd[5]*C4;
-    OMcp27_35 = qd[5]*S4;
-    OMcp27_16 = qd[4]+qd[6]*S5;
-    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
-    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
-    RLcp27_17 = ROcp27_46*dpt[2][2];
-    RLcp27_27 = ROcp27_56*dpt[2][2];
-    RLcp27_37 = ROcp27_66*dpt[2][2];
-    POcp27_17 = q[1]+RLcp27_17;
-    POcp27_27 = q[2]+RLcp27_27;
-    POcp27_37 = q[3]+RLcp27_37;
-    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
-    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
-    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
-    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
-    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
-    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
-    VIcp27_17 = qd[1]+ORcp27_17;
-    VIcp27_27 = qd[2]+ORcp27_27;
-    VIcp27_37 = qd[3]+ORcp27_37;
-    RLcp27_18 = ROcp27_46*dpt[2][6];
-    RLcp27_28 = ROcp27_56*dpt[2][6];
-    RLcp27_38 = ROcp27_66*dpt[2][6];
-    POcp27_18 = POcp27_17+RLcp27_18;
-    POcp27_28 = POcp27_27+RLcp27_28;
-    POcp27_38 = POcp27_37+RLcp27_38;
-    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
-    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
-    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
-    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
-    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
-    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
-    VIcp27_18 = ORcp27_18+VIcp27_17;
-    VIcp27_28 = ORcp27_28+VIcp27_27;
-    VIcp27_38 = ORcp27_38+VIcp27_37;
-    RLcp27_19 = ROcp27_78*dpt[3][8];
-    RLcp27_29 = ROcp27_88*dpt[3][8];
-    RLcp27_39 = ROcp27_98*dpt[3][8];
-    POcp27_19 = POcp27_18+RLcp27_19;
-    POcp27_29 = POcp27_28+RLcp27_29;
-    POcp27_39 = POcp27_38+RLcp27_39;
-    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
-    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
-    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
-    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
-    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
-    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
-    VIcp27_19 = ORcp27_19+VIcp27_18;
-    VIcp27_29 = ORcp27_29+VIcp27_28;
-    VIcp27_39 = ORcp27_39+VIcp27_38;
-    RLcp27_110 = ROcp27_78*dpt[3][11];
-    RLcp27_210 = ROcp27_88*dpt[3][11];
-    RLcp27_310 = ROcp27_98*dpt[3][11];
-    POcp27_110 = POcp27_19+RLcp27_110;
-    POcp27_210 = POcp27_29+RLcp27_210;
-    POcp27_310 = POcp27_39+RLcp27_310;
-    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
-    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
-    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
-    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
-    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
-    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
-    VIcp27_110 = ORcp27_110+VIcp27_19;
-    VIcp27_210 = ORcp27_210+VIcp27_29;
-    VIcp27_310 = ORcp27_310+VIcp27_39;
-    PxF3[1] = POcp27_110;
-    PxF3[2] = POcp27_210;
-    PxF3[3] = POcp27_310;
-    RxF3[1][1] = ROcp27_110;
-    RxF3[1][2] = ROcp27_210;
-    RxF3[1][3] = ROcp27_310;
-    RxF3[2][1] = ROcp27_49;
-    RxF3[2][2] = ROcp27_59;
-    RxF3[2][3] = ROcp27_69;
-    RxF3[3][1] = ROcp27_710;
-    RxF3[3][2] = ROcp27_810;
-    RxF3[3][3] = ROcp27_910;
-    VxF3[1] = VIcp27_110;
-    VxF3[2] = VIcp27_210;
-    VxF3[3] = VIcp27_310;
-    OMxF3[1] = OMcp27_110;
-    OMxF3[2] = OMcp27_210;
-    OMxF3[3] = OMcp27_310;
-    ROcp28_25 = S4*S5;
-    ROcp28_35 = -C4*S5;
-    ROcp28_85 = -S4*C5;
-    ROcp28_95 = C4*C5;
-    ROcp28_16 = C5*C6;
-    ROcp28_26 = ROcp28_25*C6+C4*S6;
-    ROcp28_36 = ROcp28_35*C6+S4*S6;
-    ROcp28_46 = -C5*S6;
-    ROcp28_56 = -ROcp28_25*S6+C4*C6;
-    ROcp28_66 = -ROcp28_35*S6+S4*C6;
-    ROcp28_17 = ROcp28_16*C7-S5*S7;
-    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
-    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
-    ROcp28_77 = ROcp28_16*S7+S5*C7;
-    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
-    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
-    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
-    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
-    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
-    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
-    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
-    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
-    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
-    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
-    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
-    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
-    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
-    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
-    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
-    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
-    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
-    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
-    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
-    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
-    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
-    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
-    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
-    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
-    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
-    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
-    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
-    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
-    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
-    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
-    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
-    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
-    OMcp28_25 = qd[5]*C4;
-    OMcp28_35 = qd[5]*S4;
-    OMcp28_16 = qd[4]+qd[6]*S5;
-    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
-    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
-    RLcp28_17 = ROcp28_46*dpt[2][2];
-    RLcp28_27 = ROcp28_56*dpt[2][2];
-    RLcp28_37 = ROcp28_66*dpt[2][2];
-    POcp28_17 = q[1]+RLcp28_17;
-    POcp28_27 = q[2]+RLcp28_27;
-    POcp28_37 = q[3]+RLcp28_37;
-    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
-    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
-    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
-    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
-    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
-    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
-    VIcp28_17 = qd[1]+ORcp28_17;
-    VIcp28_27 = qd[2]+ORcp28_27;
-    VIcp28_37 = qd[3]+ORcp28_37;
-    RLcp28_18 = ROcp28_46*dpt[2][6];
-    RLcp28_28 = ROcp28_56*dpt[2][6];
-    RLcp28_38 = ROcp28_66*dpt[2][6];
-    POcp28_18 = POcp28_17+RLcp28_18;
-    POcp28_28 = POcp28_27+RLcp28_28;
-    POcp28_38 = POcp28_37+RLcp28_38;
-    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
-    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
-    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
-    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
-    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
-    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
-    VIcp28_18 = ORcp28_18+VIcp28_17;
-    VIcp28_28 = ORcp28_28+VIcp28_27;
-    VIcp28_38 = ORcp28_38+VIcp28_37;
-    RLcp28_19 = ROcp28_78*dpt[3][8];
-    RLcp28_29 = ROcp28_88*dpt[3][8];
-    RLcp28_39 = ROcp28_98*dpt[3][8];
-    POcp28_19 = POcp28_18+RLcp28_19;
-    POcp28_29 = POcp28_28+RLcp28_29;
-    POcp28_39 = POcp28_38+RLcp28_39;
-    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
-    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
-    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
-    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
-    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
-    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
-    VIcp28_19 = ORcp28_19+VIcp28_18;
-    VIcp28_29 = ORcp28_29+VIcp28_28;
-    VIcp28_39 = ORcp28_39+VIcp28_38;
-    RLcp28_110 = ROcp28_78*dpt[3][11];
-    RLcp28_210 = ROcp28_88*dpt[3][11];
-    RLcp28_310 = ROcp28_98*dpt[3][11];
-    POcp28_110 = POcp28_19+RLcp28_110;
-    POcp28_210 = POcp28_29+RLcp28_210;
-    POcp28_310 = POcp28_39+RLcp28_310;
-    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
-    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
-    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
-    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
-    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
-    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
-    VIcp28_110 = ORcp28_110+VIcp28_19;
-    VIcp28_210 = ORcp28_210+VIcp28_29;
-    VIcp28_310 = ORcp28_310+VIcp28_39;
-    RLcp28_111 = ROcp28_710*dpt[3][14];
-    RLcp28_211 = ROcp28_810*dpt[3][14];
-    RLcp28_311 = ROcp28_910*dpt[3][14];
-    POcp28_111 = POcp28_110+RLcp28_111;
-    POcp28_211 = POcp28_210+RLcp28_211;
-    POcp28_311 = POcp28_310+RLcp28_311;
-    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
-    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
-    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
-    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
-    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
-    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
-    VIcp28_111 = ORcp28_111+VIcp28_110;
-    VIcp28_211 = ORcp28_211+VIcp28_210;
-    VIcp28_311 = ORcp28_311+VIcp28_310;
-    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
-    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
-    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
-    RLcp28_113 = ROcp28_712*dpt[3][18];
-    RLcp28_213 = ROcp28_812*dpt[3][18];
-    RLcp28_313 = ROcp28_912*dpt[3][18];
-    POcp28_113 = POcp28_111+RLcp28_113;
-    POcp28_213 = POcp28_211+RLcp28_213;
-    POcp28_313 = POcp28_311+RLcp28_313;
-    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
-    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
-    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
-    VIcp28_113 = ORcp28_113+VIcp28_111;
-    VIcp28_213 = ORcp28_213+VIcp28_211;
-    VIcp28_313 = ORcp28_313+VIcp28_311;
-    PxF4[1] = POcp28_113;
-    PxF4[2] = POcp28_213;
-    PxF4[3] = POcp28_313;
-    RxF4[1][1] = ROcp28_112;
-    RxF4[1][2] = ROcp28_212;
-    RxF4[1][3] = ROcp28_312;
-    RxF4[2][1] = ROcp28_411;
-    RxF4[2][2] = ROcp28_511;
-    RxF4[2][3] = ROcp28_611;
-    RxF4[3][1] = ROcp28_712;
-    RxF4[3][2] = ROcp28_812;
-    RxF4[3][3] = ROcp28_912;
-    VxF4[1] = VIcp28_113;
-    VxF4[2] = VIcp28_213;
-    VxF4[3] = VIcp28_313;
-    OMxF4[1] = OMcp28_112;
-    OMxF4[2] = OMcp28_212;
-    OMxF4[3] = OMcp28_312;
-    ROcp29_25 = S4*S5;
-    ROcp29_35 = -C4*S5;
-    ROcp29_85 = -S4*C5;
-    ROcp29_95 = C4*C5;
-    ROcp29_16 = C5*C6;
-    ROcp29_26 = ROcp29_25*C6+C4*S6;
-    ROcp29_36 = ROcp29_35*C6+S4*S6;
-    ROcp29_46 = -C5*S6;
-    ROcp29_56 = -ROcp29_25*S6+C4*C6;
-    ROcp29_66 = -ROcp29_35*S6+S4*C6;
-    ROcp29_113 = ROcp29_16*C13-S13*S5;
-    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
-    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
-    ROcp29_713 = ROcp29_16*S13+C13*S5;
-    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
-    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
-    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
-    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
-    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
-    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
-    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
-    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
-    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
-    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
-    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
-    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
-    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
-    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
-    OMcp29_25 = qd[5]*C4;
-    OMcp29_35 = qd[5]*S4;
-    OMcp29_16 = qd[4]+qd[6]*S5;
-    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
-    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
-    RLcp29_17 = ROcp29_46*dpt[2][3];
-    RLcp29_27 = ROcp29_56*dpt[2][3];
-    RLcp29_37 = ROcp29_66*dpt[2][3];
-    POcp29_17 = q[1]+RLcp29_17;
-    POcp29_27 = q[2]+RLcp29_27;
-    POcp29_37 = q[3]+RLcp29_37;
-    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
-    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
-    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
-    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
-    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
-    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
-    VIcp29_17 = qd[1]+ORcp29_17;
-    VIcp29_27 = qd[2]+ORcp29_27;
-    VIcp29_37 = qd[3]+ORcp29_37;
-    RLcp29_18 = ROcp29_46*dpt[2][20];
-    RLcp29_28 = ROcp29_56*dpt[2][20];
-    RLcp29_38 = ROcp29_66*dpt[2][20];
-    POcp29_18 = POcp29_17+RLcp29_18;
-    POcp29_28 = POcp29_27+RLcp29_28;
-    POcp29_38 = POcp29_37+RLcp29_38;
-    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
-    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
-    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
-    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
-    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
-    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
-    VIcp29_18 = ORcp29_18+VIcp29_17;
-    VIcp29_28 = ORcp29_28+VIcp29_27;
-    VIcp29_38 = ORcp29_38+VIcp29_37;
-    RLcp29_19 = ROcp29_714*dpt[3][22];
-    RLcp29_29 = ROcp29_814*dpt[3][22];
-    RLcp29_39 = ROcp29_914*dpt[3][22];
-    POcp29_19 = POcp29_18+RLcp29_19;
-    POcp29_29 = POcp29_28+RLcp29_29;
-    POcp29_39 = POcp29_38+RLcp29_39;
-    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
-    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
-    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
-    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
-    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
-    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
-    VIcp29_19 = ORcp29_19+VIcp29_18;
-    VIcp29_29 = ORcp29_29+VIcp29_28;
-    VIcp29_39 = ORcp29_39+VIcp29_38;
-    PxF5[1] = POcp29_19;
-    PxF5[2] = POcp29_29;
-    PxF5[3] = POcp29_39;
-    RxF5[1][1] = ROcp29_115;
-    RxF5[1][2] = ROcp29_215;
-    RxF5[1][3] = ROcp29_315;
-    RxF5[2][1] = ROcp29_415;
-    RxF5[2][2] = ROcp29_515;
-    RxF5[2][3] = ROcp29_615;
-    RxF5[3][1] = ROcp29_714;
-    RxF5[3][2] = ROcp29_814;
-    RxF5[3][3] = ROcp29_914;
-    VxF5[1] = VIcp29_19;
-    VxF5[2] = VIcp29_29;
-    VxF5[3] = VIcp29_39;
-    OMxF5[1] = OMcp29_19;
-    OMxF5[2] = OMcp29_29;
-    OMxF5[3] = OMcp29_39;
-    ROcp30_25 = S4*S5;
-    ROcp30_35 = -C4*S5;
-    ROcp30_85 = -S4*C5;
-    ROcp30_95 = C4*C5;
-    ROcp30_16 = C5*C6;
-    ROcp30_26 = ROcp30_25*C6+C4*S6;
-    ROcp30_36 = ROcp30_35*C6+S4*S6;
-    ROcp30_46 = -C5*S6;
-    ROcp30_56 = -ROcp30_25*S6+C4*C6;
-    ROcp30_66 = -ROcp30_35*S6+S4*C6;
-    ROcp30_113 = ROcp30_16*C13-S13*S5;
-    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
-    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
-    ROcp30_713 = ROcp30_16*S13+C13*S5;
-    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
-    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
-    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
-    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
-    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
-    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
-    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
-    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
-    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
-    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
-    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
-    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
-    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
-    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
-    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
-    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
-    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
-    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
-    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
-    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
-    OMcp30_25 = qd[5]*C4;
-    OMcp30_35 = qd[5]*S4;
-    OMcp30_16 = qd[4]+qd[6]*S5;
-    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
-    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
-    RLcp30_17 = ROcp30_46*dpt[2][3];
-    RLcp30_27 = ROcp30_56*dpt[2][3];
-    RLcp30_37 = ROcp30_66*dpt[2][3];
-    POcp30_17 = q[1]+RLcp30_17;
-    POcp30_27 = q[2]+RLcp30_27;
-    POcp30_37 = q[3]+RLcp30_37;
-    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
-    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
-    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
-    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
-    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
-    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
-    VIcp30_17 = qd[1]+ORcp30_17;
-    VIcp30_27 = qd[2]+ORcp30_27;
-    VIcp30_37 = qd[3]+ORcp30_37;
-    RLcp30_18 = ROcp30_46*dpt[2][20];
-    RLcp30_28 = ROcp30_56*dpt[2][20];
-    RLcp30_38 = ROcp30_66*dpt[2][20];
-    POcp30_18 = POcp30_17+RLcp30_18;
-    POcp30_28 = POcp30_27+RLcp30_28;
-    POcp30_38 = POcp30_37+RLcp30_38;
-    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
-    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
-    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
-    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
-    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
-    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
-    VIcp30_18 = ORcp30_18+VIcp30_17;
-    VIcp30_28 = ORcp30_28+VIcp30_27;
-    VIcp30_38 = ORcp30_38+VIcp30_37;
-    RLcp30_19 = ROcp30_714*dpt[3][22];
-    RLcp30_29 = ROcp30_814*dpt[3][22];
-    RLcp30_39 = ROcp30_914*dpt[3][22];
-    POcp30_19 = POcp30_18+RLcp30_19;
-    POcp30_29 = POcp30_28+RLcp30_29;
-    POcp30_39 = POcp30_38+RLcp30_39;
-    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
-    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
-    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
-    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
-    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
-    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
-    VIcp30_19 = ORcp30_19+VIcp30_18;
-    VIcp30_29 = ORcp30_29+VIcp30_28;
-    VIcp30_39 = ORcp30_39+VIcp30_38;
-    RLcp30_110 = ROcp30_714*dpt[3][25];
-    RLcp30_210 = ROcp30_814*dpt[3][25];
-    RLcp30_310 = ROcp30_914*dpt[3][25];
-    POcp30_110 = POcp30_19+RLcp30_110;
-    POcp30_210 = POcp30_29+RLcp30_210;
-    POcp30_310 = POcp30_39+RLcp30_310;
-    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
-    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
-    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
-    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
-    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
-    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
-    VIcp30_110 = ORcp30_110+VIcp30_19;
-    VIcp30_210 = ORcp30_210+VIcp30_29;
-    VIcp30_310 = ORcp30_310+VIcp30_39;
-    PxF6[1] = POcp30_110;
-    PxF6[2] = POcp30_210;
-    PxF6[3] = POcp30_310;
-    RxF6[1][1] = ROcp30_116;
-    RxF6[1][2] = ROcp30_216;
-    RxF6[1][3] = ROcp30_316;
-    RxF6[2][1] = ROcp30_415;
-    RxF6[2][2] = ROcp30_515;
-    RxF6[2][3] = ROcp30_615;
-    RxF6[3][1] = ROcp30_716;
-    RxF6[3][2] = ROcp30_816;
-    RxF6[3][3] = ROcp30_916;
-    VxF6[1] = VIcp30_110;
-    VxF6[2] = VIcp30_210;
-    VxF6[3] = VIcp30_310;
-    OMxF6[1] = OMcp30_110;
-    OMxF6[2] = OMcp30_210;
-    OMxF6[3] = OMcp30_310;
-    ROcp31_25 = S4*S5;
-    ROcp31_35 = -C4*S5;
-    ROcp31_85 = -S4*C5;
-    ROcp31_95 = C4*C5;
-    ROcp31_16 = C5*C6;
-    ROcp31_26 = ROcp31_25*C6+C4*S6;
-    ROcp31_36 = ROcp31_35*C6+S4*S6;
-    ROcp31_46 = -C5*S6;
-    ROcp31_56 = -ROcp31_25*S6+C4*C6;
-    ROcp31_66 = -ROcp31_35*S6+S4*C6;
-    ROcp31_113 = ROcp31_16*C13-S13*S5;
-    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
-    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
-    ROcp31_713 = ROcp31_16*S13+C13*S5;
-    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
-    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
-    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
-    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
-    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
-    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
-    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
-    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
-    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
-    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
-    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
-    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
-    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
-    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
-    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
-    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
-    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
-    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
-    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
-    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
-    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
-    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
-    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
-    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
-    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
-    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
-    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
-    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
-    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
-    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
-    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
-    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
-    OMcp31_25 = qd[5]*C4;
-    OMcp31_35 = qd[5]*S4;
-    OMcp31_16 = qd[4]+qd[6]*S5;
-    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
-    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
-    RLcp31_17 = ROcp31_46*dpt[2][3];
-    RLcp31_27 = ROcp31_56*dpt[2][3];
-    RLcp31_37 = ROcp31_66*dpt[2][3];
-    POcp31_17 = q[1]+RLcp31_17;
-    POcp31_27 = q[2]+RLcp31_27;
-    POcp31_37 = q[3]+RLcp31_37;
-    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
-    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
-    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
-    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
-    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
-    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
-    VIcp31_17 = qd[1]+ORcp31_17;
-    VIcp31_27 = qd[2]+ORcp31_27;
-    VIcp31_37 = qd[3]+ORcp31_37;
-    RLcp31_18 = ROcp31_46*dpt[2][20];
-    RLcp31_28 = ROcp31_56*dpt[2][20];
-    RLcp31_38 = ROcp31_66*dpt[2][20];
-    POcp31_18 = POcp31_17+RLcp31_18;
-    POcp31_28 = POcp31_27+RLcp31_28;
-    POcp31_38 = POcp31_37+RLcp31_38;
-    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
-    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
-    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
-    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
-    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
-    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
-    VIcp31_18 = ORcp31_18+VIcp31_17;
-    VIcp31_28 = ORcp31_28+VIcp31_27;
-    VIcp31_38 = ORcp31_38+VIcp31_37;
-    RLcp31_19 = ROcp31_714*dpt[3][22];
-    RLcp31_29 = ROcp31_814*dpt[3][22];
-    RLcp31_39 = ROcp31_914*dpt[3][22];
-    POcp31_19 = POcp31_18+RLcp31_19;
-    POcp31_29 = POcp31_28+RLcp31_29;
-    POcp31_39 = POcp31_38+RLcp31_39;
-    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
-    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
-    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
-    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
-    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
-    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
-    VIcp31_19 = ORcp31_19+VIcp31_18;
-    VIcp31_29 = ORcp31_29+VIcp31_28;
-    VIcp31_39 = ORcp31_39+VIcp31_38;
-    RLcp31_110 = ROcp31_714*dpt[3][25];
-    RLcp31_210 = ROcp31_814*dpt[3][25];
-    RLcp31_310 = ROcp31_914*dpt[3][25];
-    POcp31_110 = POcp31_19+RLcp31_110;
-    POcp31_210 = POcp31_29+RLcp31_210;
-    POcp31_310 = POcp31_39+RLcp31_310;
-    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
-    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
-    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
-    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
-    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
-    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
-    VIcp31_110 = ORcp31_110+VIcp31_19;
-    VIcp31_210 = ORcp31_210+VIcp31_29;
-    VIcp31_310 = ORcp31_310+VIcp31_39;
-    RLcp31_111 = ROcp31_716*dpt[3][28];
-    RLcp31_211 = ROcp31_816*dpt[3][28];
-    RLcp31_311 = ROcp31_916*dpt[3][28];
-    POcp31_111 = POcp31_110+RLcp31_111;
-    POcp31_211 = POcp31_210+RLcp31_211;
-    POcp31_311 = POcp31_310+RLcp31_311;
-    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
-    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
-    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
-    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
-    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
-    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
-    VIcp31_111 = ORcp31_111+VIcp31_110;
-    VIcp31_211 = ORcp31_211+VIcp31_210;
-    VIcp31_311 = ORcp31_311+VIcp31_310;
-    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
-    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
-    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
-    RLcp31_113 = ROcp31_718*dpt[3][33];
-    RLcp31_213 = ROcp31_818*dpt[3][33];
-    RLcp31_313 = ROcp31_918*dpt[3][33];
-    POcp31_113 = POcp31_111+RLcp31_113;
-    POcp31_213 = POcp31_211+RLcp31_213;
-    POcp31_313 = POcp31_311+RLcp31_313;
-    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
-    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
-    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
-    VIcp31_113 = ORcp31_113+VIcp31_111;
-    VIcp31_213 = ORcp31_213+VIcp31_211;
-    VIcp31_313 = ORcp31_313+VIcp31_311;
-    PxF7[1] = POcp31_113;
-    PxF7[2] = POcp31_213;
-    PxF7[3] = POcp31_313;
-    RxF7[1][1] = ROcp31_118;
-    RxF7[1][2] = ROcp31_218;
-    RxF7[1][3] = ROcp31_318;
-    RxF7[2][1] = ROcp31_417;
-    RxF7[2][2] = ROcp31_517;
-    RxF7[2][3] = ROcp31_617;
-    RxF7[3][1] = ROcp31_718;
-    RxF7[3][2] = ROcp31_818;
-    RxF7[3][3] = ROcp31_918;
-    VxF7[1] = VIcp31_113;
-    VxF7[2] = VIcp31_213;
-    VxF7[3] = VIcp31_313;
-    OMxF7[1] = OMcp31_112;
-    OMxF7[2] = OMcp31_212;
-    OMxF7[3] = OMcp31_312;
-    ROcp32_25 = S4*S5;
-    ROcp32_35 = -C4*S5;
-    ROcp32_85 = -S4*C5;
-    ROcp32_95 = C4*C5;
-    ROcp32_16 = C5*C6;
-    ROcp32_26 = ROcp32_25*C6+C4*S6;
-    ROcp32_36 = ROcp32_35*C6+S4*S6;
-    ROcp32_46 = -C5*S6;
-    ROcp32_56 = -ROcp32_25*S6+C4*C6;
-    ROcp32_66 = -ROcp32_35*S6+S4*C6;
-    ROcp32_419 = ROcp32_46*C19+S19*S5;
-    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
-    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
-    ROcp32_719 = -ROcp32_46*S19+C19*S5;
-    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
-    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
-    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
-    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
-    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
-    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
-    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
-    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
-    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
-    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
-    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
-    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
-    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
-    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
-    OMcp32_25 = qd[5]*C4;
-    OMcp32_35 = qd[5]*S4;
-    OMcp32_16 = qd[4]+qd[6]*S5;
-    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
-    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
-    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
-    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
-    POcp32_17 = q[1]+RLcp32_17;
-    POcp32_27 = q[2]+RLcp32_27;
-    POcp32_37 = q[3]+RLcp32_37;
-    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
-    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
-    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
-    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
-    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
-    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
-    VIcp32_17 = qd[1]+ORcp32_17;
-    VIcp32_27 = qd[2]+ORcp32_27;
-    VIcp32_37 = qd[3]+ORcp32_37;
-    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
-    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
-    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
-    RLcp32_19 = ROcp32_720*dpt[3][36];
-    RLcp32_29 = ROcp32_820*dpt[3][36];
-    RLcp32_39 = ROcp32_920*dpt[3][36];
-    POcp32_19 = POcp32_17+RLcp32_19;
-    POcp32_29 = POcp32_27+RLcp32_29;
-    POcp32_39 = POcp32_37+RLcp32_39;
-    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
-    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
-    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
-    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
-    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
-    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
-    VIcp32_19 = ORcp32_19+VIcp32_17;
-    VIcp32_29 = ORcp32_29+VIcp32_27;
-    VIcp32_39 = ORcp32_39+VIcp32_37;
-    PxF8[1] = POcp32_19;
-    PxF8[2] = POcp32_29;
-    PxF8[3] = POcp32_39;
-    RxF8[1][1] = ROcp32_121;
-    RxF8[1][2] = ROcp32_221;
-    RxF8[1][3] = ROcp32_321;
-    RxF8[2][1] = ROcp32_421;
-    RxF8[2][2] = ROcp32_521;
-    RxF8[2][3] = ROcp32_621;
-    RxF8[3][1] = ROcp32_720;
-    RxF8[3][2] = ROcp32_820;
-    RxF8[3][3] = ROcp32_920;
-    VxF8[1] = VIcp32_19;
-    VxF8[2] = VIcp32_29;
-    VxF8[3] = VIcp32_39;
-    OMxF8[1] = OMcp32_19;
-    OMxF8[2] = OMcp32_29;
-    OMxF8[3] = OMcp32_39;
-    ROcp33_25 = S4*S5;
-    ROcp33_35 = -C4*S5;
-    ROcp33_85 = -S4*C5;
-    ROcp33_95 = C4*C5;
-    ROcp33_16 = C5*C6;
-    ROcp33_26 = ROcp33_25*C6+C4*S6;
-    ROcp33_36 = ROcp33_35*C6+S4*S6;
-    ROcp33_46 = -C5*S6;
-    ROcp33_56 = -ROcp33_25*S6+C4*C6;
-    ROcp33_66 = -ROcp33_35*S6+S4*C6;
-    ROcp33_419 = ROcp33_46*C19+S19*S5;
-    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
-    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
-    ROcp33_719 = -ROcp33_46*S19+C19*S5;
-    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
-    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
-    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
-    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
-    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
-    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
-    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
-    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
-    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
-    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
-    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
-    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
-    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
-    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
-    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
-    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
-    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
-    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
-    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
-    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
-    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
-    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
-    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
-    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
-    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
-    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
-    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
-    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
-    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
-    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
-    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
-    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
-    OMcp33_25 = qd[5]*C4;
-    OMcp33_35 = qd[5]*S4;
-    OMcp33_16 = qd[4]+qd[6]*S5;
-    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
-    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
-    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
-    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
-    POcp33_17 = q[1]+RLcp33_17;
-    POcp33_27 = q[2]+RLcp33_27;
-    POcp33_37 = q[3]+RLcp33_37;
-    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
-    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
-    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
-    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
-    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
-    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
-    VIcp33_17 = qd[1]+ORcp33_17;
-    VIcp33_27 = qd[2]+ORcp33_27;
-    VIcp33_37 = qd[3]+ORcp33_37;
-    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
-    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
-    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
-    RLcp33_19 = ROcp33_720*dpt[3][36];
-    RLcp33_29 = ROcp33_820*dpt[3][36];
-    RLcp33_39 = ROcp33_920*dpt[3][36];
-    POcp33_19 = POcp33_17+RLcp33_19;
-    POcp33_29 = POcp33_27+RLcp33_29;
-    POcp33_39 = POcp33_37+RLcp33_39;
-    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
-    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
-    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
-    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
-    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
-    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
-    VIcp33_19 = ORcp33_19+VIcp33_17;
-    VIcp33_29 = ORcp33_29+VIcp33_27;
-    VIcp33_39 = ORcp33_39+VIcp33_37;
-    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
-    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
-    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
-    POcp33_110 = POcp33_19+RLcp33_110;
-    POcp33_210 = POcp33_29+RLcp33_210;
-    POcp33_310 = POcp33_39+RLcp33_310;
-    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
-    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
-    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
-    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
-    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
-    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
-    VIcp33_110 = ORcp33_110+VIcp33_19;
-    VIcp33_210 = ORcp33_210+VIcp33_29;
-    VIcp33_310 = ORcp33_310+VIcp33_39;
-    RLcp33_111 = ROcp33_421*dpt[2][43];
-    RLcp33_211 = ROcp33_521*dpt[2][43];
-    RLcp33_311 = ROcp33_621*dpt[2][43];
-    POcp33_111 = POcp33_110+RLcp33_111;
-    POcp33_211 = POcp33_210+RLcp33_211;
-    POcp33_311 = POcp33_310+RLcp33_311;
-    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
-    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
-    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
-    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
-    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
-    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
-    VIcp33_111 = ORcp33_111+VIcp33_110;
-    VIcp33_211 = ORcp33_211+VIcp33_210;
-    VIcp33_311 = ORcp33_311+VIcp33_310;
-    RLcp33_112 = ROcp33_723*dpt[3][45];
-    RLcp33_212 = ROcp33_823*dpt[3][45];
-    RLcp33_312 = ROcp33_923*dpt[3][45];
-    POcp33_112 = POcp33_111+RLcp33_112;
-    POcp33_212 = POcp33_211+RLcp33_212;
-    POcp33_312 = POcp33_311+RLcp33_312;
-    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
-    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
-    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
-    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
-    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
-    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
-    VIcp33_112 = ORcp33_112+VIcp33_111;
-    VIcp33_212 = ORcp33_212+VIcp33_211;
-    VIcp33_312 = ORcp33_312+VIcp33_311;
-    PxF9[1] = POcp33_112;
-    PxF9[2] = POcp33_212;
-    PxF9[3] = POcp33_312;
-    RxF9[1][1] = ROcp33_124;
-    RxF9[1][2] = ROcp33_224;
-    RxF9[1][3] = ROcp33_324;
-    RxF9[2][1] = ROcp33_424;
-    RxF9[2][2] = ROcp33_524;
-    RxF9[2][3] = ROcp33_624;
-    RxF9[3][1] = ROcp33_723;
-    RxF9[3][2] = ROcp33_823;
-    RxF9[3][3] = ROcp33_923;
-    VxF9[1] = VIcp33_112;
-    VxF9[2] = VIcp33_212;
-    VxF9[3] = VIcp33_312;
-    OMxF9[1] = OMcp33_112;
-    OMxF9[2] = OMcp33_212;
-    OMxF9[3] = OMcp33_312;
-    ROcp34_25 = S4*S5;
-    ROcp34_35 = -C4*S5;
-    ROcp34_85 = -S4*C5;
-    ROcp34_95 = C4*C5;
-    ROcp34_16 = C5*C6;
-    ROcp34_26 = ROcp34_25*C6+C4*S6;
-    ROcp34_36 = ROcp34_35*C6+S4*S6;
-    ROcp34_46 = -C5*S6;
-    ROcp34_56 = -ROcp34_25*S6+C4*C6;
-    ROcp34_66 = -ROcp34_35*S6+S4*C6;
-    ROcp34_419 = ROcp34_46*C19+S19*S5;
-    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
-    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
-    ROcp34_719 = -ROcp34_46*S19+C19*S5;
-    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
-    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
-    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
-    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
-    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
-    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
-    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
-    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
-    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
-    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
-    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
-    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
-    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
-    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
-    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
-    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
-    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
-    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
-    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
-    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
-    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
-    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
-    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
-    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
-    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
-    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
-    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
-    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
-    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
-    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
-    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
-    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
-    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
-    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
-    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
-    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
-    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
-    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
-    OMcp34_25 = qd[5]*C4;
-    OMcp34_35 = qd[5]*S4;
-    OMcp34_16 = qd[4]+qd[6]*S5;
-    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
-    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
-    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
-    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
-    POcp34_17 = q[1]+RLcp34_17;
-    POcp34_27 = q[2]+RLcp34_27;
-    POcp34_37 = q[3]+RLcp34_37;
-    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
-    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
-    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
-    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
-    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
-    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
-    VIcp34_17 = qd[1]+ORcp34_17;
-    VIcp34_27 = qd[2]+ORcp34_27;
-    VIcp34_37 = qd[3]+ORcp34_37;
-    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
-    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
-    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
-    RLcp34_19 = ROcp34_720*dpt[3][36];
-    RLcp34_29 = ROcp34_820*dpt[3][36];
-    RLcp34_39 = ROcp34_920*dpt[3][36];
-    POcp34_19 = POcp34_17+RLcp34_19;
-    POcp34_29 = POcp34_27+RLcp34_29;
-    POcp34_39 = POcp34_37+RLcp34_39;
-    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
-    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
-    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
-    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
-    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
-    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
-    VIcp34_19 = ORcp34_19+VIcp34_17;
-    VIcp34_29 = ORcp34_29+VIcp34_27;
-    VIcp34_39 = ORcp34_39+VIcp34_37;
-    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
-    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
-    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
-    POcp34_110 = POcp34_19+RLcp34_110;
-    POcp34_210 = POcp34_29+RLcp34_210;
-    POcp34_310 = POcp34_39+RLcp34_310;
-    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
-    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
-    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
-    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
-    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
-    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
-    VIcp34_110 = ORcp34_110+VIcp34_19;
-    VIcp34_210 = ORcp34_210+VIcp34_29;
-    VIcp34_310 = ORcp34_310+VIcp34_39;
-    RLcp34_111 = ROcp34_421*dpt[2][43];
-    RLcp34_211 = ROcp34_521*dpt[2][43];
-    RLcp34_311 = ROcp34_621*dpt[2][43];
-    POcp34_111 = POcp34_110+RLcp34_111;
-    POcp34_211 = POcp34_210+RLcp34_211;
-    POcp34_311 = POcp34_310+RLcp34_311;
-    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
-    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
-    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
-    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
-    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
-    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
-    VIcp34_111 = ORcp34_111+VIcp34_110;
-    VIcp34_211 = ORcp34_211+VIcp34_210;
-    VIcp34_311 = ORcp34_311+VIcp34_310;
-    RLcp34_112 = ROcp34_723*dpt[3][45];
-    RLcp34_212 = ROcp34_823*dpt[3][45];
-    RLcp34_312 = ROcp34_923*dpt[3][45];
-    POcp34_112 = POcp34_111+RLcp34_112;
-    POcp34_212 = POcp34_211+RLcp34_212;
-    POcp34_312 = POcp34_311+RLcp34_312;
-    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
-    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
-    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
-    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
-    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
-    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
-    VIcp34_112 = ORcp34_112+VIcp34_111;
-    VIcp34_212 = ORcp34_212+VIcp34_211;
-    VIcp34_312 = ORcp34_312+VIcp34_311;
-    RLcp34_113 = ROcp34_723*dpt[3][48];
-    RLcp34_213 = ROcp34_823*dpt[3][48];
-    RLcp34_313 = ROcp34_923*dpt[3][48];
-    POcp34_113 = POcp34_112+RLcp34_113;
-    POcp34_213 = POcp34_212+RLcp34_213;
-    POcp34_313 = POcp34_312+RLcp34_313;
-    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
-    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
-    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
-    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
-    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
-    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
-    VIcp34_113 = ORcp34_113+VIcp34_112;
-    VIcp34_213 = ORcp34_213+VIcp34_212;
-    VIcp34_313 = ORcp34_313+VIcp34_312;
-    PxF10[1] = POcp34_113;
-    PxF10[2] = POcp34_213;
-    PxF10[3] = POcp34_313;
-    RxF10[1][1] = ROcp34_125;
-    RxF10[1][2] = ROcp34_225;
-    RxF10[1][3] = ROcp34_325;
-    RxF10[2][1] = ROcp34_424;
-    RxF10[2][2] = ROcp34_524;
-    RxF10[2][3] = ROcp34_624;
-    RxF10[3][1] = ROcp34_725;
-    RxF10[3][2] = ROcp34_825;
-    RxF10[3][3] = ROcp34_925;
-    VxF10[1] = VIcp34_113;
-    VxF10[2] = VIcp34_213;
-    VxF10[3] = VIcp34_313;
-    OMxF10[1] = OMcp34_113;
-    OMxF10[2] = OMcp34_213;
-    OMxF10[3] = OMcp34_313;
-    ROcp35_25 = S4*S5;
-    ROcp35_35 = -C4*S5;
-    ROcp35_85 = -S4*C5;
-    ROcp35_95 = C4*C5;
-    ROcp35_16 = C5*C6;
-    ROcp35_26 = ROcp35_25*C6+C4*S6;
-    ROcp35_36 = ROcp35_35*C6+S4*S6;
-    ROcp35_46 = -C5*S6;
-    ROcp35_56 = -ROcp35_25*S6+C4*C6;
-    ROcp35_66 = -ROcp35_35*S6+S4*C6;
-    ROcp35_419 = ROcp35_46*C19+S19*S5;
-    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
-    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
-    ROcp35_719 = -ROcp35_46*S19+C19*S5;
-    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
-    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
-    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
-    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
-    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
-    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
-    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
-    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
-    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
-    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
-    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
-    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
-    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
-    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
-    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
-    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
-    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
-    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
-    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
-    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
-    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
-    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
-    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
-    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
-    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
-    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
-    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
-    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
-    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
-    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
-    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
-    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
-    OMcp35_25 = qd[5]*C4;
-    OMcp35_35 = qd[5]*S4;
-    OMcp35_16 = qd[4]+qd[6]*S5;
-    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
-    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
-    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
-    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
-    POcp35_17 = q[1]+RLcp35_17;
-    POcp35_27 = q[2]+RLcp35_27;
-    POcp35_37 = q[3]+RLcp35_37;
-    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
-    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
-    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
-    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
-    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
-    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
-    VIcp35_17 = qd[1]+ORcp35_17;
-    VIcp35_27 = qd[2]+ORcp35_27;
-    VIcp35_37 = qd[3]+ORcp35_37;
-    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
-    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
-    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
-    RLcp35_19 = ROcp35_720*dpt[3][36];
-    RLcp35_29 = ROcp35_820*dpt[3][36];
-    RLcp35_39 = ROcp35_920*dpt[3][36];
-    POcp35_19 = POcp35_17+RLcp35_19;
-    POcp35_29 = POcp35_27+RLcp35_29;
-    POcp35_39 = POcp35_37+RLcp35_39;
-    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
-    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
-    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
-    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
-    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
-    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
-    VIcp35_19 = ORcp35_19+VIcp35_17;
-    VIcp35_29 = ORcp35_29+VIcp35_27;
-    VIcp35_39 = ORcp35_39+VIcp35_37;
-    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
-    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
-    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
-    POcp35_110 = POcp35_19+RLcp35_110;
-    POcp35_210 = POcp35_29+RLcp35_210;
-    POcp35_310 = POcp35_39+RLcp35_310;
-    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
-    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
-    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
-    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
-    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
-    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
-    VIcp35_110 = ORcp35_110+VIcp35_19;
-    VIcp35_210 = ORcp35_210+VIcp35_29;
-    VIcp35_310 = ORcp35_310+VIcp35_39;
-    RLcp35_111 = ROcp35_421*dpt[2][52];
-    RLcp35_211 = ROcp35_521*dpt[2][52];
-    RLcp35_311 = ROcp35_621*dpt[2][52];
-    POcp35_111 = POcp35_110+RLcp35_111;
-    POcp35_211 = POcp35_210+RLcp35_211;
-    POcp35_311 = POcp35_310+RLcp35_311;
-    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
-    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
-    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
-    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
-    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
-    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
-    VIcp35_111 = ORcp35_111+VIcp35_110;
-    VIcp35_211 = ORcp35_211+VIcp35_210;
-    VIcp35_311 = ORcp35_311+VIcp35_310;
-    RLcp35_112 = ROcp35_727*dpt[3][54];
-    RLcp35_212 = ROcp35_827*dpt[3][54];
-    RLcp35_312 = ROcp35_927*dpt[3][54];
-    POcp35_112 = POcp35_111+RLcp35_112;
-    POcp35_212 = POcp35_211+RLcp35_212;
-    POcp35_312 = POcp35_311+RLcp35_312;
-    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
-    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
-    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
-    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
-    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
-    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
-    VIcp35_112 = ORcp35_112+VIcp35_111;
-    VIcp35_212 = ORcp35_212+VIcp35_211;
-    VIcp35_312 = ORcp35_312+VIcp35_311;
-    PxF11[1] = POcp35_112;
-    PxF11[2] = POcp35_212;
-    PxF11[3] = POcp35_312;
-    RxF11[1][1] = ROcp35_128;
-    RxF11[1][2] = ROcp35_228;
-    RxF11[1][3] = ROcp35_328;
-    RxF11[2][1] = ROcp35_428;
-    RxF11[2][2] = ROcp35_528;
-    RxF11[2][3] = ROcp35_628;
-    RxF11[3][1] = ROcp35_727;
-    RxF11[3][2] = ROcp35_827;
-    RxF11[3][3] = ROcp35_927;
-    VxF11[1] = VIcp35_112;
-    VxF11[2] = VIcp35_212;
-    VxF11[3] = VIcp35_312;
-    OMxF11[1] = OMcp35_112;
-    OMxF11[2] = OMcp35_212;
-    OMxF11[3] = OMcp35_312;
-    ROcp36_25 = S4*S5;
-    ROcp36_35 = -C4*S5;
-    ROcp36_85 = -S4*C5;
-    ROcp36_95 = C4*C5;
-    ROcp36_16 = C5*C6;
-    ROcp36_26 = ROcp36_25*C6+C4*S6;
-    ROcp36_36 = ROcp36_35*C6+S4*S6;
-    ROcp36_46 = -C5*S6;
-    ROcp36_56 = -ROcp36_25*S6+C4*C6;
-    ROcp36_66 = -ROcp36_35*S6+S4*C6;
-    ROcp36_419 = ROcp36_46*C19+S19*S5;
-    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
-    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
-    ROcp36_719 = -ROcp36_46*S19+C19*S5;
-    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
-    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
-    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
-    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
-    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
-    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
-    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
-    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
-    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
-    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
-    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
-    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
-    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
-    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
-    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
-    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
-    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
-    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
-    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
-    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
-    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
-    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
-    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
-    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
-    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
-    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
-    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
-    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
-    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
-    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
-    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
-    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
-    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
-    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
-    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
-    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
-    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
-    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
-    OMcp36_25 = qd[5]*C4;
-    OMcp36_35 = qd[5]*S4;
-    OMcp36_16 = qd[4]+qd[6]*S5;
-    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
-    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
-    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
-    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
-    POcp36_17 = q[1]+RLcp36_17;
-    POcp36_27 = q[2]+RLcp36_27;
-    POcp36_37 = q[3]+RLcp36_37;
-    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
-    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
-    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
-    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
-    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
-    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
-    VIcp36_17 = qd[1]+ORcp36_17;
-    VIcp36_27 = qd[2]+ORcp36_27;
-    VIcp36_37 = qd[3]+ORcp36_37;
-    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
-    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
-    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
-    RLcp36_19 = ROcp36_720*dpt[3][36];
-    RLcp36_29 = ROcp36_820*dpt[3][36];
-    RLcp36_39 = ROcp36_920*dpt[3][36];
-    POcp36_19 = POcp36_17+RLcp36_19;
-    POcp36_29 = POcp36_27+RLcp36_29;
-    POcp36_39 = POcp36_37+RLcp36_39;
-    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
-    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
-    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
-    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
-    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
-    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
-    VIcp36_19 = ORcp36_19+VIcp36_17;
-    VIcp36_29 = ORcp36_29+VIcp36_27;
-    VIcp36_39 = ORcp36_39+VIcp36_37;
-    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
-    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
-    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
-    POcp36_110 = POcp36_19+RLcp36_110;
-    POcp36_210 = POcp36_29+RLcp36_210;
-    POcp36_310 = POcp36_39+RLcp36_310;
-    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
-    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
-    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
-    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
-    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
-    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
-    VIcp36_110 = ORcp36_110+VIcp36_19;
-    VIcp36_210 = ORcp36_210+VIcp36_29;
-    VIcp36_310 = ORcp36_310+VIcp36_39;
-    RLcp36_111 = ROcp36_421*dpt[2][52];
-    RLcp36_211 = ROcp36_521*dpt[2][52];
-    RLcp36_311 = ROcp36_621*dpt[2][52];
-    POcp36_111 = POcp36_110+RLcp36_111;
-    POcp36_211 = POcp36_210+RLcp36_211;
-    POcp36_311 = POcp36_310+RLcp36_311;
-    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
-    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
-    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
-    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
-    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
-    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
-    VIcp36_111 = ORcp36_111+VIcp36_110;
-    VIcp36_211 = ORcp36_211+VIcp36_210;
-    VIcp36_311 = ORcp36_311+VIcp36_310;
-    RLcp36_112 = ROcp36_727*dpt[3][54];
-    RLcp36_212 = ROcp36_827*dpt[3][54];
-    RLcp36_312 = ROcp36_927*dpt[3][54];
-    POcp36_112 = POcp36_111+RLcp36_112;
-    POcp36_212 = POcp36_211+RLcp36_212;
-    POcp36_312 = POcp36_311+RLcp36_312;
-    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
-    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
-    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
-    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
-    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
-    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
-    VIcp36_112 = ORcp36_112+VIcp36_111;
-    VIcp36_212 = ORcp36_212+VIcp36_211;
-    VIcp36_312 = ORcp36_312+VIcp36_311;
-    RLcp36_113 = ROcp36_727*dpt[3][57];
-    RLcp36_213 = ROcp36_827*dpt[3][57];
-    RLcp36_313 = ROcp36_927*dpt[3][57];
-    POcp36_113 = POcp36_112+RLcp36_113;
-    POcp36_213 = POcp36_212+RLcp36_213;
-    POcp36_313 = POcp36_312+RLcp36_313;
-    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
-    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
-    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
-    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
-    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
-    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
-    VIcp36_113 = ORcp36_113+VIcp36_112;
-    VIcp36_213 = ORcp36_213+VIcp36_212;
-    VIcp36_313 = ORcp36_313+VIcp36_312;
-    PxF12[1] = POcp36_113;
-    PxF12[2] = POcp36_213;
-    PxF12[3] = POcp36_313;
-    RxF12[1][1] = ROcp36_129;
-    RxF12[1][2] = ROcp36_229;
-    RxF12[1][3] = ROcp36_329;
-    RxF12[2][1] = ROcp36_428;
-    RxF12[2][2] = ROcp36_528;
-    RxF12[2][3] = ROcp36_628;
-    RxF12[3][1] = ROcp36_729;
-    RxF12[3][2] = ROcp36_829;
-    RxF12[3][3] = ROcp36_929;
-    VxF12[1] = VIcp36_113;
-    VxF12[2] = VIcp36_213;
-    VxF12[3] = VIcp36_313;
-    OMxF12[1] = OMcp36_113;
-    OMxF12[2] = OMcp36_213;
-    OMxF12[3] = OMcp36_313;
- 
-    // output copy
-    for(i=1; i<=3; i++)
-    {
-        PxF_tab[0][i] = PxF1[i];
-        PxF_tab[1][i] = PxF2[i];
-        PxF_tab[2][i] = PxF3[i];
-        PxF_tab[3][i] = PxF4[i];
-        PxF_tab[4][i] = PxF5[i];
-        PxF_tab[5][i] = PxF6[i];
-        PxF_tab[6][i] = PxF7[i];
-        PxF_tab[7][i] = PxF8[i];
-        PxF_tab[8][i] = PxF9[i];
-        PxF_tab[9][i] = PxF10[i];
-        PxF_tab[10][i] = PxF11[i];
-        PxF_tab[11][i] = PxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        for(j=1; j<=3; j++)
-        {
-        RxF_tab[0][i][j] = RxF1[i][j];
-        RxF_tab[1][i][j] = RxF2[i][j];
-        RxF_tab[2][i][j] = RxF3[i][j];
-        RxF_tab[3][i][j] = RxF4[i][j];
-        RxF_tab[4][i][j] = RxF5[i][j];
-        RxF_tab[5][i][j] = RxF6[i][j];
-        RxF_tab[6][i][j] = RxF7[i][j];
-        RxF_tab[7][i][j] = RxF8[i][j];
-        RxF_tab[8][i][j] = RxF9[i][j];
-        RxF_tab[9][i][j] = RxF10[i][j];
-        RxF_tab[10][i][j] = RxF11[i][j];
-        RxF_tab[11][i][j] = RxF12[i][j];
-        }
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        VxF_tab[0][i] = VxF1[i];
-        VxF_tab[1][i] = VxF2[i];
-        VxF_tab[2][i] = VxF3[i];
-        VxF_tab[3][i] = VxF4[i];
-        VxF_tab[4][i] = VxF5[i];
-        VxF_tab[5][i] = VxF6[i];
-        VxF_tab[6][i] = VxF7[i];
-        VxF_tab[7][i] = VxF8[i];
-        VxF_tab[8][i] = VxF9[i];
-        VxF_tab[9][i] = VxF10[i];
-        VxF_tab[10][i] = VxF11[i];
-        VxF_tab[11][i] = VxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        OMxF_tab[0][i] = OMxF1[i];
-        OMxF_tab[1][i] = OMxF2[i];
-        OMxF_tab[2][i] = OMxF3[i];
-        OMxF_tab[3][i] = OMxF4[i];
-        OMxF_tab[4][i] = OMxF5[i];
-        OMxF_tab[5][i] = OMxF6[i];
-        OMxF_tab[6][i] = OMxF7[i];
-        OMxF_tab[7][i] = OMxF8[i];
-        OMxF_tab[8][i] = OMxF9[i];
-        OMxF_tab[9][i] = OMxF10[i];
-        OMxF_tab[10][i] = OMxF11[i];
-        OMxF_tab[11][i] = OMxF12[i];
-    }
-
-}
diff --git a/symbolicR/long_feet/CMakeLists.txt b/symbolicR/long_feet/CMakeLists.txt
index 3bae37b..c963c74 100644
--- a/symbolicR/long_feet/CMakeLists.txt
+++ b/symbolicR/long_feet/CMakeLists.txt
@@ -1,12 +1,22 @@
-# Copyright: (C)
-# Authors: Timothee Habra & Nicolas Van der Noot
-# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# CMake for compiling the symbolic files of a robotran project in C
+# 
 
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 #                       LIBRARY MAIN CONFIGURATIONS
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
-cmake_minimum_required(VERSION 2.8.7)
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
 
 project(Project_symbolic)
 
@@ -49,10 +59,26 @@ include_directories ( ${INCLUDE_DIR} )
 include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
 
 if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
+
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
     add_library(Project_symbolic SHARED ${SOURCE_FILES})
+	
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_symbolic PROPERTIES SUFFIX .so)    
+    endif()    
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
+        add_custom_command(TARGET Project_symbolic POST_BUILD
+			COMMAND ${CMAKE_COMMAND} -E copy
+                        ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_symbolic.dll
+                        ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_symbolic.dll
+        )
+    endif()
+
 else()
     add_library(Project_symbolic STATIC ${SOURCE_FILES})
-    add_definitions( -DPROJECT_USERFCT_STATIC_DEFINE)
 
     # some symbolic routines call user functions (ext_forces, accelred, ...)
     target_link_libraries(Project_symbolic Project_userfct)
@@ -67,14 +93,5 @@ endif()
 if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
     find_package( LibRobotranC REQUIRED )
     add_definitions(${LIB_MBSYSC_DEFINITIONS})
+    target_link_libraries(Project_symbolic ${LIB_MBSYSC_MODULES})
 endif()
-
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         STUFF FOR WINDOWS (TO BE CLEANED)
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-include (GenerateExportHeader)
-GENERATE_EXPORT_HEADER( Project_symbolic    )
-
-install (TARGETS Project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
diff --git a/symbolicR/long_feet/user_kinematics_model.c b/symbolicR/long_feet/user_kinematics_model.c
deleted file mode 100644
index 542f5ca..0000000
--- a/symbolicR/long_feet/user_kinematics_model.c
+++ /dev/null
@@ -1,1734 +0,0 @@
-#include "mbs_data.h"
-
-#include "math.h"
-
-/*! \brief compute the kinematics model of sensors with ExtForces
- * 
- * \param[in] mbs_data Robotran structure
- * \param[out] PxF_tab tabular with all the 'PxF' computed
- * \param[out] RxF_tab tabular with all the 'RxF' computed
- * \param[out] VxF_tab tabular with all the 'VxF' computed
- * \param[out] OMxF_tab tabular with all the 'OMxF' computed
- */
-void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
-{
-    // variables declaration
-    int i, j;
-    double *q, *qd;
-    double **dpt;
-
-    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
-    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
-    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
-    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
-
-    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
-    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
-
-    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
-    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
-    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
-    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
-    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
-    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
-    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
-    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
-    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
-    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
-    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
-    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
-    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
-    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
-    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
-    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
-    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
-    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
-    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
-    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
-    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
-    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
-    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
-    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
-    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
-    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
-    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
-    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
-    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
-    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
-    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
-    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
-    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
-    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
-    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
-    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
-    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
-    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
-    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
-    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
-    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
-    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
-    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
-    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
-    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
-    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
-    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
-    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
-    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
-    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
-    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
-    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
-    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
-    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
-    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
-    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
-    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
-    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
-    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
-    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
-    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
-    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
-    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
-    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
-    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
-    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
-    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
-    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
-    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
-    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
-    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
-    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
-    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
-    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
-    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
-    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
-    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
-    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
-    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
-    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
-    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
-    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
-    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
-    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
-    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
-
-    // variables initialization
-    q   = mbs_data->q;
-    qd  = mbs_data->qd;
-    dpt = mbs_data->dpt;
-
-    // cosines and sines
-    C4 = cos(q[4]);
-    C5 = cos(q[5]);
-    C6 = cos(q[6]);
-    C7 = cos(q[7]);
-    C8 = cos(q[8]);
-    C9 = cos(q[9]);
-    C10 = cos(q[10]);
-    C11 = cos(q[11]);
-    C12 = cos(q[12]);
-    C13 = cos(q[13]);
-    C14 = cos(q[14]);
-    C15 = cos(q[15]);
-    C16 = cos(q[16]);
-    C17 = cos(q[17]);
-    C18 = cos(q[18]);
-    C19 = cos(q[19]);
-    C20 = cos(q[20]);
-    C21 = cos(q[21]);
-    C22 = cos(q[22]);
-    C23 = cos(q[23]);
-    C24 = cos(q[24]);
-    C25 = cos(q[25]);
-    C26 = cos(q[26]);
-    C27 = cos(q[27]);
-    C28 = cos(q[28]);
-    C29 = cos(q[29]);
-    S4 = sin(q[4]);
-    S5 = sin(q[5]);
-    S6 = sin(q[6]);
-    S7 = sin(q[7]);
-    S8 = sin(q[8]);
-    S9 = sin(q[9]);
-    S10 = sin(q[10]);
-    S11 = sin(q[11]);
-    S12 = sin(q[12]);
-    S13 = sin(q[13]);
-    S14 = sin(q[14]);
-    S15 = sin(q[15]);
-    S16 = sin(q[16]);
-    S17 = sin(q[17]);
-    S18 = sin(q[18]);
-    S19 = sin(q[19]);
-    S20 = sin(q[20]);
-    S21 = sin(q[21]);
-    S22 = sin(q[22]);
-    S23 = sin(q[23]);
-    S24 = sin(q[24]);
-    S25 = sin(q[25]);
-    S26 = sin(q[26]);
-    S27 = sin(q[27]);
-    S28 = sin(q[28]);
-    S29 = sin(q[29]);
-
-    // main computation
-    ROcp25_25 = S4*S5;
-    ROcp25_35 = -C4*S5;
-    ROcp25_85 = -S4*C5;
-    ROcp25_95 = C4*C5;
-    ROcp25_16 = C5*C6;
-    ROcp25_26 = ROcp25_25*C6+C4*S6;
-    ROcp25_36 = ROcp25_35*C6+S4*S6;
-    ROcp25_46 = -C5*S6;
-    ROcp25_56 = -ROcp25_25*S6+C4*C6;
-    ROcp25_66 = -ROcp25_35*S6+S4*C6;
-    OMcp25_25 = qd[5]*C4;
-    OMcp25_35 = qd[5]*S4;
-    OMcp25_16 = qd[4]+qd[6]*S5;
-    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
-    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
-    PxF1[1] = q[1];
-    PxF1[2] = q[2];
-    PxF1[3] = q[3];
-    RxF1[1][1] = ROcp25_16;
-    RxF1[1][2] = ROcp25_26;
-    RxF1[1][3] = ROcp25_36;
-    RxF1[2][1] = ROcp25_46;
-    RxF1[2][2] = ROcp25_56;
-    RxF1[2][3] = ROcp25_66;
-    RxF1[3][1] = S5;
-    RxF1[3][2] = ROcp25_85;
-    RxF1[3][3] = ROcp25_95;
-    VxF1[1] = qd[1];
-    VxF1[2] = qd[2];
-    VxF1[3] = qd[3];
-    OMxF1[1] = OMcp25_16;
-    OMxF1[2] = OMcp25_26;
-    OMxF1[3] = OMcp25_36;
-    ROcp26_25 = S4*S5;
-    ROcp26_35 = -C4*S5;
-    ROcp26_85 = -S4*C5;
-    ROcp26_95 = C4*C5;
-    ROcp26_16 = C5*C6;
-    ROcp26_26 = ROcp26_25*C6+C4*S6;
-    ROcp26_36 = ROcp26_35*C6+S4*S6;
-    ROcp26_46 = -C5*S6;
-    ROcp26_56 = -ROcp26_25*S6+C4*C6;
-    ROcp26_66 = -ROcp26_35*S6+S4*C6;
-    ROcp26_17 = ROcp26_16*C7-S5*S7;
-    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
-    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
-    ROcp26_77 = ROcp26_16*S7+S5*C7;
-    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
-    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
-    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
-    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
-    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
-    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
-    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
-    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
-    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
-    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
-    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
-    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
-    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
-    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
-    OMcp26_25 = qd[5]*C4;
-    OMcp26_35 = qd[5]*S4;
-    OMcp26_16 = qd[4]+qd[6]*S5;
-    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
-    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
-    RLcp26_17 = ROcp26_46*dpt[2][2];
-    RLcp26_27 = ROcp26_56*dpt[2][2];
-    RLcp26_37 = ROcp26_66*dpt[2][2];
-    POcp26_17 = q[1]+RLcp26_17;
-    POcp26_27 = q[2]+RLcp26_27;
-    POcp26_37 = q[3]+RLcp26_37;
-    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
-    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
-    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
-    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
-    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
-    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
-    VIcp26_17 = qd[1]+ORcp26_17;
-    VIcp26_27 = qd[2]+ORcp26_27;
-    VIcp26_37 = qd[3]+ORcp26_37;
-    RLcp26_18 = ROcp26_46*dpt[2][6];
-    RLcp26_28 = ROcp26_56*dpt[2][6];
-    RLcp26_38 = ROcp26_66*dpt[2][6];
-    POcp26_18 = POcp26_17+RLcp26_18;
-    POcp26_28 = POcp26_27+RLcp26_28;
-    POcp26_38 = POcp26_37+RLcp26_38;
-    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
-    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
-    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
-    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
-    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
-    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
-    VIcp26_18 = ORcp26_18+VIcp26_17;
-    VIcp26_28 = ORcp26_28+VIcp26_27;
-    VIcp26_38 = ORcp26_38+VIcp26_37;
-    RLcp26_19 = ROcp26_78*dpt[3][8];
-    RLcp26_29 = ROcp26_88*dpt[3][8];
-    RLcp26_39 = ROcp26_98*dpt[3][8];
-    POcp26_19 = POcp26_18+RLcp26_19;
-    POcp26_29 = POcp26_28+RLcp26_29;
-    POcp26_39 = POcp26_38+RLcp26_39;
-    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
-    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
-    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
-    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
-    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
-    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
-    VIcp26_19 = ORcp26_19+VIcp26_18;
-    VIcp26_29 = ORcp26_29+VIcp26_28;
-    VIcp26_39 = ORcp26_39+VIcp26_38;
-    PxF2[1] = POcp26_19;
-    PxF2[2] = POcp26_29;
-    PxF2[3] = POcp26_39;
-    RxF2[1][1] = ROcp26_19;
-    RxF2[1][2] = ROcp26_29;
-    RxF2[1][3] = ROcp26_39;
-    RxF2[2][1] = ROcp26_49;
-    RxF2[2][2] = ROcp26_59;
-    RxF2[2][3] = ROcp26_69;
-    RxF2[3][1] = ROcp26_78;
-    RxF2[3][2] = ROcp26_88;
-    RxF2[3][3] = ROcp26_98;
-    VxF2[1] = VIcp26_19;
-    VxF2[2] = VIcp26_29;
-    VxF2[3] = VIcp26_39;
-    OMxF2[1] = OMcp26_19;
-    OMxF2[2] = OMcp26_29;
-    OMxF2[3] = OMcp26_39;
-    ROcp27_25 = S4*S5;
-    ROcp27_35 = -C4*S5;
-    ROcp27_85 = -S4*C5;
-    ROcp27_95 = C4*C5;
-    ROcp27_16 = C5*C6;
-    ROcp27_26 = ROcp27_25*C6+C4*S6;
-    ROcp27_36 = ROcp27_35*C6+S4*S6;
-    ROcp27_46 = -C5*S6;
-    ROcp27_56 = -ROcp27_25*S6+C4*C6;
-    ROcp27_66 = -ROcp27_35*S6+S4*C6;
-    ROcp27_17 = ROcp27_16*C7-S5*S7;
-    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
-    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
-    ROcp27_77 = ROcp27_16*S7+S5*C7;
-    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
-    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
-    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
-    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
-    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
-    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
-    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
-    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
-    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
-    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
-    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
-    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
-    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
-    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
-    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
-    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
-    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
-    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
-    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
-    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
-    OMcp27_25 = qd[5]*C4;
-    OMcp27_35 = qd[5]*S4;
-    OMcp27_16 = qd[4]+qd[6]*S5;
-    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
-    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
-    RLcp27_17 = ROcp27_46*dpt[2][2];
-    RLcp27_27 = ROcp27_56*dpt[2][2];
-    RLcp27_37 = ROcp27_66*dpt[2][2];
-    POcp27_17 = q[1]+RLcp27_17;
-    POcp27_27 = q[2]+RLcp27_27;
-    POcp27_37 = q[3]+RLcp27_37;
-    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
-    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
-    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
-    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
-    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
-    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
-    VIcp27_17 = qd[1]+ORcp27_17;
-    VIcp27_27 = qd[2]+ORcp27_27;
-    VIcp27_37 = qd[3]+ORcp27_37;
-    RLcp27_18 = ROcp27_46*dpt[2][6];
-    RLcp27_28 = ROcp27_56*dpt[2][6];
-    RLcp27_38 = ROcp27_66*dpt[2][6];
-    POcp27_18 = POcp27_17+RLcp27_18;
-    POcp27_28 = POcp27_27+RLcp27_28;
-    POcp27_38 = POcp27_37+RLcp27_38;
-    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
-    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
-    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
-    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
-    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
-    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
-    VIcp27_18 = ORcp27_18+VIcp27_17;
-    VIcp27_28 = ORcp27_28+VIcp27_27;
-    VIcp27_38 = ORcp27_38+VIcp27_37;
-    RLcp27_19 = ROcp27_78*dpt[3][8];
-    RLcp27_29 = ROcp27_88*dpt[3][8];
-    RLcp27_39 = ROcp27_98*dpt[3][8];
-    POcp27_19 = POcp27_18+RLcp27_19;
-    POcp27_29 = POcp27_28+RLcp27_29;
-    POcp27_39 = POcp27_38+RLcp27_39;
-    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
-    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
-    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
-    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
-    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
-    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
-    VIcp27_19 = ORcp27_19+VIcp27_18;
-    VIcp27_29 = ORcp27_29+VIcp27_28;
-    VIcp27_39 = ORcp27_39+VIcp27_38;
-    RLcp27_110 = ROcp27_78*dpt[3][11];
-    RLcp27_210 = ROcp27_88*dpt[3][11];
-    RLcp27_310 = ROcp27_98*dpt[3][11];
-    POcp27_110 = POcp27_19+RLcp27_110;
-    POcp27_210 = POcp27_29+RLcp27_210;
-    POcp27_310 = POcp27_39+RLcp27_310;
-    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
-    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
-    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
-    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
-    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
-    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
-    VIcp27_110 = ORcp27_110+VIcp27_19;
-    VIcp27_210 = ORcp27_210+VIcp27_29;
-    VIcp27_310 = ORcp27_310+VIcp27_39;
-    PxF3[1] = POcp27_110;
-    PxF3[2] = POcp27_210;
-    PxF3[3] = POcp27_310;
-    RxF3[1][1] = ROcp27_110;
-    RxF3[1][2] = ROcp27_210;
-    RxF3[1][3] = ROcp27_310;
-    RxF3[2][1] = ROcp27_49;
-    RxF3[2][2] = ROcp27_59;
-    RxF3[2][3] = ROcp27_69;
-    RxF3[3][1] = ROcp27_710;
-    RxF3[3][2] = ROcp27_810;
-    RxF3[3][3] = ROcp27_910;
-    VxF3[1] = VIcp27_110;
-    VxF3[2] = VIcp27_210;
-    VxF3[3] = VIcp27_310;
-    OMxF3[1] = OMcp27_110;
-    OMxF3[2] = OMcp27_210;
-    OMxF3[3] = OMcp27_310;
-    ROcp28_25 = S4*S5;
-    ROcp28_35 = -C4*S5;
-    ROcp28_85 = -S4*C5;
-    ROcp28_95 = C4*C5;
-    ROcp28_16 = C5*C6;
-    ROcp28_26 = ROcp28_25*C6+C4*S6;
-    ROcp28_36 = ROcp28_35*C6+S4*S6;
-    ROcp28_46 = -C5*S6;
-    ROcp28_56 = -ROcp28_25*S6+C4*C6;
-    ROcp28_66 = -ROcp28_35*S6+S4*C6;
-    ROcp28_17 = ROcp28_16*C7-S5*S7;
-    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
-    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
-    ROcp28_77 = ROcp28_16*S7+S5*C7;
-    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
-    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
-    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
-    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
-    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
-    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
-    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
-    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
-    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
-    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
-    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
-    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
-    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
-    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
-    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
-    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
-    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
-    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
-    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
-    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
-    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
-    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
-    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
-    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
-    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
-    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
-    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
-    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
-    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
-    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
-    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
-    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
-    OMcp28_25 = qd[5]*C4;
-    OMcp28_35 = qd[5]*S4;
-    OMcp28_16 = qd[4]+qd[6]*S5;
-    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
-    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
-    RLcp28_17 = ROcp28_46*dpt[2][2];
-    RLcp28_27 = ROcp28_56*dpt[2][2];
-    RLcp28_37 = ROcp28_66*dpt[2][2];
-    POcp28_17 = q[1]+RLcp28_17;
-    POcp28_27 = q[2]+RLcp28_27;
-    POcp28_37 = q[3]+RLcp28_37;
-    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
-    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
-    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
-    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
-    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
-    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
-    VIcp28_17 = qd[1]+ORcp28_17;
-    VIcp28_27 = qd[2]+ORcp28_27;
-    VIcp28_37 = qd[3]+ORcp28_37;
-    RLcp28_18 = ROcp28_46*dpt[2][6];
-    RLcp28_28 = ROcp28_56*dpt[2][6];
-    RLcp28_38 = ROcp28_66*dpt[2][6];
-    POcp28_18 = POcp28_17+RLcp28_18;
-    POcp28_28 = POcp28_27+RLcp28_28;
-    POcp28_38 = POcp28_37+RLcp28_38;
-    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
-    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
-    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
-    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
-    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
-    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
-    VIcp28_18 = ORcp28_18+VIcp28_17;
-    VIcp28_28 = ORcp28_28+VIcp28_27;
-    VIcp28_38 = ORcp28_38+VIcp28_37;
-    RLcp28_19 = ROcp28_78*dpt[3][8];
-    RLcp28_29 = ROcp28_88*dpt[3][8];
-    RLcp28_39 = ROcp28_98*dpt[3][8];
-    POcp28_19 = POcp28_18+RLcp28_19;
-    POcp28_29 = POcp28_28+RLcp28_29;
-    POcp28_39 = POcp28_38+RLcp28_39;
-    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
-    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
-    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
-    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
-    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
-    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
-    VIcp28_19 = ORcp28_19+VIcp28_18;
-    VIcp28_29 = ORcp28_29+VIcp28_28;
-    VIcp28_39 = ORcp28_39+VIcp28_38;
-    RLcp28_110 = ROcp28_78*dpt[3][11];
-    RLcp28_210 = ROcp28_88*dpt[3][11];
-    RLcp28_310 = ROcp28_98*dpt[3][11];
-    POcp28_110 = POcp28_19+RLcp28_110;
-    POcp28_210 = POcp28_29+RLcp28_210;
-    POcp28_310 = POcp28_39+RLcp28_310;
-    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
-    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
-    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
-    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
-    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
-    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
-    VIcp28_110 = ORcp28_110+VIcp28_19;
-    VIcp28_210 = ORcp28_210+VIcp28_29;
-    VIcp28_310 = ORcp28_310+VIcp28_39;
-    RLcp28_111 = ROcp28_710*dpt[3][14];
-    RLcp28_211 = ROcp28_810*dpt[3][14];
-    RLcp28_311 = ROcp28_910*dpt[3][14];
-    POcp28_111 = POcp28_110+RLcp28_111;
-    POcp28_211 = POcp28_210+RLcp28_211;
-    POcp28_311 = POcp28_310+RLcp28_311;
-    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
-    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
-    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
-    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
-    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
-    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
-    VIcp28_111 = ORcp28_111+VIcp28_110;
-    VIcp28_211 = ORcp28_211+VIcp28_210;
-    VIcp28_311 = ORcp28_311+VIcp28_310;
-    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
-    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
-    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
-    RLcp28_113 = ROcp28_712*dpt[3][18];
-    RLcp28_213 = ROcp28_812*dpt[3][18];
-    RLcp28_313 = ROcp28_912*dpt[3][18];
-    POcp28_113 = POcp28_111+RLcp28_113;
-    POcp28_213 = POcp28_211+RLcp28_213;
-    POcp28_313 = POcp28_311+RLcp28_313;
-    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
-    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
-    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
-    VIcp28_113 = ORcp28_113+VIcp28_111;
-    VIcp28_213 = ORcp28_213+VIcp28_211;
-    VIcp28_313 = ORcp28_313+VIcp28_311;
-    PxF4[1] = POcp28_113;
-    PxF4[2] = POcp28_213;
-    PxF4[3] = POcp28_313;
-    RxF4[1][1] = ROcp28_112;
-    RxF4[1][2] = ROcp28_212;
-    RxF4[1][3] = ROcp28_312;
-    RxF4[2][1] = ROcp28_411;
-    RxF4[2][2] = ROcp28_511;
-    RxF4[2][3] = ROcp28_611;
-    RxF4[3][1] = ROcp28_712;
-    RxF4[3][2] = ROcp28_812;
-    RxF4[3][3] = ROcp28_912;
-    VxF4[1] = VIcp28_113;
-    VxF4[2] = VIcp28_213;
-    VxF4[3] = VIcp28_313;
-    OMxF4[1] = OMcp28_112;
-    OMxF4[2] = OMcp28_212;
-    OMxF4[3] = OMcp28_312;
-    ROcp29_25 = S4*S5;
-    ROcp29_35 = -C4*S5;
-    ROcp29_85 = -S4*C5;
-    ROcp29_95 = C4*C5;
-    ROcp29_16 = C5*C6;
-    ROcp29_26 = ROcp29_25*C6+C4*S6;
-    ROcp29_36 = ROcp29_35*C6+S4*S6;
-    ROcp29_46 = -C5*S6;
-    ROcp29_56 = -ROcp29_25*S6+C4*C6;
-    ROcp29_66 = -ROcp29_35*S6+S4*C6;
-    ROcp29_113 = ROcp29_16*C13-S13*S5;
-    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
-    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
-    ROcp29_713 = ROcp29_16*S13+C13*S5;
-    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
-    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
-    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
-    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
-    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
-    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
-    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
-    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
-    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
-    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
-    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
-    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
-    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
-    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
-    OMcp29_25 = qd[5]*C4;
-    OMcp29_35 = qd[5]*S4;
-    OMcp29_16 = qd[4]+qd[6]*S5;
-    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
-    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
-    RLcp29_17 = ROcp29_46*dpt[2][3];
-    RLcp29_27 = ROcp29_56*dpt[2][3];
-    RLcp29_37 = ROcp29_66*dpt[2][3];
-    POcp29_17 = q[1]+RLcp29_17;
-    POcp29_27 = q[2]+RLcp29_27;
-    POcp29_37 = q[3]+RLcp29_37;
-    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
-    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
-    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
-    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
-    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
-    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
-    VIcp29_17 = qd[1]+ORcp29_17;
-    VIcp29_27 = qd[2]+ORcp29_27;
-    VIcp29_37 = qd[3]+ORcp29_37;
-    RLcp29_18 = ROcp29_46*dpt[2][20];
-    RLcp29_28 = ROcp29_56*dpt[2][20];
-    RLcp29_38 = ROcp29_66*dpt[2][20];
-    POcp29_18 = POcp29_17+RLcp29_18;
-    POcp29_28 = POcp29_27+RLcp29_28;
-    POcp29_38 = POcp29_37+RLcp29_38;
-    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
-    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
-    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
-    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
-    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
-    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
-    VIcp29_18 = ORcp29_18+VIcp29_17;
-    VIcp29_28 = ORcp29_28+VIcp29_27;
-    VIcp29_38 = ORcp29_38+VIcp29_37;
-    RLcp29_19 = ROcp29_714*dpt[3][22];
-    RLcp29_29 = ROcp29_814*dpt[3][22];
-    RLcp29_39 = ROcp29_914*dpt[3][22];
-    POcp29_19 = POcp29_18+RLcp29_19;
-    POcp29_29 = POcp29_28+RLcp29_29;
-    POcp29_39 = POcp29_38+RLcp29_39;
-    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
-    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
-    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
-    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
-    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
-    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
-    VIcp29_19 = ORcp29_19+VIcp29_18;
-    VIcp29_29 = ORcp29_29+VIcp29_28;
-    VIcp29_39 = ORcp29_39+VIcp29_38;
-    PxF5[1] = POcp29_19;
-    PxF5[2] = POcp29_29;
-    PxF5[3] = POcp29_39;
-    RxF5[1][1] = ROcp29_115;
-    RxF5[1][2] = ROcp29_215;
-    RxF5[1][3] = ROcp29_315;
-    RxF5[2][1] = ROcp29_415;
-    RxF5[2][2] = ROcp29_515;
-    RxF5[2][3] = ROcp29_615;
-    RxF5[3][1] = ROcp29_714;
-    RxF5[3][2] = ROcp29_814;
-    RxF5[3][3] = ROcp29_914;
-    VxF5[1] = VIcp29_19;
-    VxF5[2] = VIcp29_29;
-    VxF5[3] = VIcp29_39;
-    OMxF5[1] = OMcp29_19;
-    OMxF5[2] = OMcp29_29;
-    OMxF5[3] = OMcp29_39;
-    ROcp30_25 = S4*S5;
-    ROcp30_35 = -C4*S5;
-    ROcp30_85 = -S4*C5;
-    ROcp30_95 = C4*C5;
-    ROcp30_16 = C5*C6;
-    ROcp30_26 = ROcp30_25*C6+C4*S6;
-    ROcp30_36 = ROcp30_35*C6+S4*S6;
-    ROcp30_46 = -C5*S6;
-    ROcp30_56 = -ROcp30_25*S6+C4*C6;
-    ROcp30_66 = -ROcp30_35*S6+S4*C6;
-    ROcp30_113 = ROcp30_16*C13-S13*S5;
-    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
-    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
-    ROcp30_713 = ROcp30_16*S13+C13*S5;
-    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
-    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
-    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
-    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
-    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
-    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
-    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
-    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
-    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
-    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
-    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
-    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
-    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
-    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
-    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
-    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
-    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
-    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
-    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
-    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
-    OMcp30_25 = qd[5]*C4;
-    OMcp30_35 = qd[5]*S4;
-    OMcp30_16 = qd[4]+qd[6]*S5;
-    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
-    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
-    RLcp30_17 = ROcp30_46*dpt[2][3];
-    RLcp30_27 = ROcp30_56*dpt[2][3];
-    RLcp30_37 = ROcp30_66*dpt[2][3];
-    POcp30_17 = q[1]+RLcp30_17;
-    POcp30_27 = q[2]+RLcp30_27;
-    POcp30_37 = q[3]+RLcp30_37;
-    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
-    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
-    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
-    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
-    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
-    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
-    VIcp30_17 = qd[1]+ORcp30_17;
-    VIcp30_27 = qd[2]+ORcp30_27;
-    VIcp30_37 = qd[3]+ORcp30_37;
-    RLcp30_18 = ROcp30_46*dpt[2][20];
-    RLcp30_28 = ROcp30_56*dpt[2][20];
-    RLcp30_38 = ROcp30_66*dpt[2][20];
-    POcp30_18 = POcp30_17+RLcp30_18;
-    POcp30_28 = POcp30_27+RLcp30_28;
-    POcp30_38 = POcp30_37+RLcp30_38;
-    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
-    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
-    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
-    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
-    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
-    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
-    VIcp30_18 = ORcp30_18+VIcp30_17;
-    VIcp30_28 = ORcp30_28+VIcp30_27;
-    VIcp30_38 = ORcp30_38+VIcp30_37;
-    RLcp30_19 = ROcp30_714*dpt[3][22];
-    RLcp30_29 = ROcp30_814*dpt[3][22];
-    RLcp30_39 = ROcp30_914*dpt[3][22];
-    POcp30_19 = POcp30_18+RLcp30_19;
-    POcp30_29 = POcp30_28+RLcp30_29;
-    POcp30_39 = POcp30_38+RLcp30_39;
-    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
-    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
-    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
-    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
-    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
-    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
-    VIcp30_19 = ORcp30_19+VIcp30_18;
-    VIcp30_29 = ORcp30_29+VIcp30_28;
-    VIcp30_39 = ORcp30_39+VIcp30_38;
-    RLcp30_110 = ROcp30_714*dpt[3][25];
-    RLcp30_210 = ROcp30_814*dpt[3][25];
-    RLcp30_310 = ROcp30_914*dpt[3][25];
-    POcp30_110 = POcp30_19+RLcp30_110;
-    POcp30_210 = POcp30_29+RLcp30_210;
-    POcp30_310 = POcp30_39+RLcp30_310;
-    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
-    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
-    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
-    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
-    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
-    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
-    VIcp30_110 = ORcp30_110+VIcp30_19;
-    VIcp30_210 = ORcp30_210+VIcp30_29;
-    VIcp30_310 = ORcp30_310+VIcp30_39;
-    PxF6[1] = POcp30_110;
-    PxF6[2] = POcp30_210;
-    PxF6[3] = POcp30_310;
-    RxF6[1][1] = ROcp30_116;
-    RxF6[1][2] = ROcp30_216;
-    RxF6[1][3] = ROcp30_316;
-    RxF6[2][1] = ROcp30_415;
-    RxF6[2][2] = ROcp30_515;
-    RxF6[2][3] = ROcp30_615;
-    RxF6[3][1] = ROcp30_716;
-    RxF6[3][2] = ROcp30_816;
-    RxF6[3][3] = ROcp30_916;
-    VxF6[1] = VIcp30_110;
-    VxF6[2] = VIcp30_210;
-    VxF6[3] = VIcp30_310;
-    OMxF6[1] = OMcp30_110;
-    OMxF6[2] = OMcp30_210;
-    OMxF6[3] = OMcp30_310;
-    ROcp31_25 = S4*S5;
-    ROcp31_35 = -C4*S5;
-    ROcp31_85 = -S4*C5;
-    ROcp31_95 = C4*C5;
-    ROcp31_16 = C5*C6;
-    ROcp31_26 = ROcp31_25*C6+C4*S6;
-    ROcp31_36 = ROcp31_35*C6+S4*S6;
-    ROcp31_46 = -C5*S6;
-    ROcp31_56 = -ROcp31_25*S6+C4*C6;
-    ROcp31_66 = -ROcp31_35*S6+S4*C6;
-    ROcp31_113 = ROcp31_16*C13-S13*S5;
-    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
-    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
-    ROcp31_713 = ROcp31_16*S13+C13*S5;
-    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
-    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
-    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
-    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
-    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
-    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
-    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
-    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
-    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
-    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
-    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
-    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
-    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
-    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
-    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
-    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
-    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
-    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
-    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
-    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
-    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
-    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
-    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
-    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
-    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
-    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
-    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
-    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
-    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
-    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
-    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
-    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
-    OMcp31_25 = qd[5]*C4;
-    OMcp31_35 = qd[5]*S4;
-    OMcp31_16 = qd[4]+qd[6]*S5;
-    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
-    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
-    RLcp31_17 = ROcp31_46*dpt[2][3];
-    RLcp31_27 = ROcp31_56*dpt[2][3];
-    RLcp31_37 = ROcp31_66*dpt[2][3];
-    POcp31_17 = q[1]+RLcp31_17;
-    POcp31_27 = q[2]+RLcp31_27;
-    POcp31_37 = q[3]+RLcp31_37;
-    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
-    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
-    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
-    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
-    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
-    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
-    VIcp31_17 = qd[1]+ORcp31_17;
-    VIcp31_27 = qd[2]+ORcp31_27;
-    VIcp31_37 = qd[3]+ORcp31_37;
-    RLcp31_18 = ROcp31_46*dpt[2][20];
-    RLcp31_28 = ROcp31_56*dpt[2][20];
-    RLcp31_38 = ROcp31_66*dpt[2][20];
-    POcp31_18 = POcp31_17+RLcp31_18;
-    POcp31_28 = POcp31_27+RLcp31_28;
-    POcp31_38 = POcp31_37+RLcp31_38;
-    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
-    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
-    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
-    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
-    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
-    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
-    VIcp31_18 = ORcp31_18+VIcp31_17;
-    VIcp31_28 = ORcp31_28+VIcp31_27;
-    VIcp31_38 = ORcp31_38+VIcp31_37;
-    RLcp31_19 = ROcp31_714*dpt[3][22];
-    RLcp31_29 = ROcp31_814*dpt[3][22];
-    RLcp31_39 = ROcp31_914*dpt[3][22];
-    POcp31_19 = POcp31_18+RLcp31_19;
-    POcp31_29 = POcp31_28+RLcp31_29;
-    POcp31_39 = POcp31_38+RLcp31_39;
-    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
-    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
-    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
-    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
-    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
-    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
-    VIcp31_19 = ORcp31_19+VIcp31_18;
-    VIcp31_29 = ORcp31_29+VIcp31_28;
-    VIcp31_39 = ORcp31_39+VIcp31_38;
-    RLcp31_110 = ROcp31_714*dpt[3][25];
-    RLcp31_210 = ROcp31_814*dpt[3][25];
-    RLcp31_310 = ROcp31_914*dpt[3][25];
-    POcp31_110 = POcp31_19+RLcp31_110;
-    POcp31_210 = POcp31_29+RLcp31_210;
-    POcp31_310 = POcp31_39+RLcp31_310;
-    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
-    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
-    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
-    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
-    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
-    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
-    VIcp31_110 = ORcp31_110+VIcp31_19;
-    VIcp31_210 = ORcp31_210+VIcp31_29;
-    VIcp31_310 = ORcp31_310+VIcp31_39;
-    RLcp31_111 = ROcp31_716*dpt[3][28];
-    RLcp31_211 = ROcp31_816*dpt[3][28];
-    RLcp31_311 = ROcp31_916*dpt[3][28];
-    POcp31_111 = POcp31_110+RLcp31_111;
-    POcp31_211 = POcp31_210+RLcp31_211;
-    POcp31_311 = POcp31_310+RLcp31_311;
-    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
-    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
-    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
-    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
-    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
-    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
-    VIcp31_111 = ORcp31_111+VIcp31_110;
-    VIcp31_211 = ORcp31_211+VIcp31_210;
-    VIcp31_311 = ORcp31_311+VIcp31_310;
-    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
-    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
-    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
-    RLcp31_113 = ROcp31_718*dpt[3][33];
-    RLcp31_213 = ROcp31_818*dpt[3][33];
-    RLcp31_313 = ROcp31_918*dpt[3][33];
-    POcp31_113 = POcp31_111+RLcp31_113;
-    POcp31_213 = POcp31_211+RLcp31_213;
-    POcp31_313 = POcp31_311+RLcp31_313;
-    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
-    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
-    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
-    VIcp31_113 = ORcp31_113+VIcp31_111;
-    VIcp31_213 = ORcp31_213+VIcp31_211;
-    VIcp31_313 = ORcp31_313+VIcp31_311;
-    PxF7[1] = POcp31_113;
-    PxF7[2] = POcp31_213;
-    PxF7[3] = POcp31_313;
-    RxF7[1][1] = ROcp31_118;
-    RxF7[1][2] = ROcp31_218;
-    RxF7[1][3] = ROcp31_318;
-    RxF7[2][1] = ROcp31_417;
-    RxF7[2][2] = ROcp31_517;
-    RxF7[2][3] = ROcp31_617;
-    RxF7[3][1] = ROcp31_718;
-    RxF7[3][2] = ROcp31_818;
-    RxF7[3][3] = ROcp31_918;
-    VxF7[1] = VIcp31_113;
-    VxF7[2] = VIcp31_213;
-    VxF7[3] = VIcp31_313;
-    OMxF7[1] = OMcp31_112;
-    OMxF7[2] = OMcp31_212;
-    OMxF7[3] = OMcp31_312;
-    ROcp32_25 = S4*S5;
-    ROcp32_35 = -C4*S5;
-    ROcp32_85 = -S4*C5;
-    ROcp32_95 = C4*C5;
-    ROcp32_16 = C5*C6;
-    ROcp32_26 = ROcp32_25*C6+C4*S6;
-    ROcp32_36 = ROcp32_35*C6+S4*S6;
-    ROcp32_46 = -C5*S6;
-    ROcp32_56 = -ROcp32_25*S6+C4*C6;
-    ROcp32_66 = -ROcp32_35*S6+S4*C6;
-    ROcp32_419 = ROcp32_46*C19+S19*S5;
-    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
-    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
-    ROcp32_719 = -ROcp32_46*S19+C19*S5;
-    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
-    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
-    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
-    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
-    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
-    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
-    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
-    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
-    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
-    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
-    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
-    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
-    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
-    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
-    OMcp32_25 = qd[5]*C4;
-    OMcp32_35 = qd[5]*S4;
-    OMcp32_16 = qd[4]+qd[6]*S5;
-    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
-    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
-    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
-    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
-    POcp32_17 = q[1]+RLcp32_17;
-    POcp32_27 = q[2]+RLcp32_27;
-    POcp32_37 = q[3]+RLcp32_37;
-    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
-    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
-    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
-    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
-    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
-    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
-    VIcp32_17 = qd[1]+ORcp32_17;
-    VIcp32_27 = qd[2]+ORcp32_27;
-    VIcp32_37 = qd[3]+ORcp32_37;
-    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
-    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
-    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
-    RLcp32_19 = ROcp32_720*dpt[3][36];
-    RLcp32_29 = ROcp32_820*dpt[3][36];
-    RLcp32_39 = ROcp32_920*dpt[3][36];
-    POcp32_19 = POcp32_17+RLcp32_19;
-    POcp32_29 = POcp32_27+RLcp32_29;
-    POcp32_39 = POcp32_37+RLcp32_39;
-    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
-    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
-    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
-    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
-    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
-    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
-    VIcp32_19 = ORcp32_19+VIcp32_17;
-    VIcp32_29 = ORcp32_29+VIcp32_27;
-    VIcp32_39 = ORcp32_39+VIcp32_37;
-    PxF8[1] = POcp32_19;
-    PxF8[2] = POcp32_29;
-    PxF8[3] = POcp32_39;
-    RxF8[1][1] = ROcp32_121;
-    RxF8[1][2] = ROcp32_221;
-    RxF8[1][3] = ROcp32_321;
-    RxF8[2][1] = ROcp32_421;
-    RxF8[2][2] = ROcp32_521;
-    RxF8[2][3] = ROcp32_621;
-    RxF8[3][1] = ROcp32_720;
-    RxF8[3][2] = ROcp32_820;
-    RxF8[3][3] = ROcp32_920;
-    VxF8[1] = VIcp32_19;
-    VxF8[2] = VIcp32_29;
-    VxF8[3] = VIcp32_39;
-    OMxF8[1] = OMcp32_19;
-    OMxF8[2] = OMcp32_29;
-    OMxF8[3] = OMcp32_39;
-    ROcp33_25 = S4*S5;
-    ROcp33_35 = -C4*S5;
-    ROcp33_85 = -S4*C5;
-    ROcp33_95 = C4*C5;
-    ROcp33_16 = C5*C6;
-    ROcp33_26 = ROcp33_25*C6+C4*S6;
-    ROcp33_36 = ROcp33_35*C6+S4*S6;
-    ROcp33_46 = -C5*S6;
-    ROcp33_56 = -ROcp33_25*S6+C4*C6;
-    ROcp33_66 = -ROcp33_35*S6+S4*C6;
-    ROcp33_419 = ROcp33_46*C19+S19*S5;
-    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
-    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
-    ROcp33_719 = -ROcp33_46*S19+C19*S5;
-    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
-    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
-    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
-    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
-    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
-    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
-    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
-    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
-    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
-    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
-    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
-    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
-    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
-    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
-    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
-    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
-    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
-    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
-    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
-    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
-    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
-    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
-    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
-    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
-    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
-    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
-    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
-    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
-    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
-    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
-    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
-    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
-    OMcp33_25 = qd[5]*C4;
-    OMcp33_35 = qd[5]*S4;
-    OMcp33_16 = qd[4]+qd[6]*S5;
-    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
-    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
-    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
-    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
-    POcp33_17 = q[1]+RLcp33_17;
-    POcp33_27 = q[2]+RLcp33_27;
-    POcp33_37 = q[3]+RLcp33_37;
-    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
-    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
-    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
-    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
-    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
-    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
-    VIcp33_17 = qd[1]+ORcp33_17;
-    VIcp33_27 = qd[2]+ORcp33_27;
-    VIcp33_37 = qd[3]+ORcp33_37;
-    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
-    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
-    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
-    RLcp33_19 = ROcp33_720*dpt[3][36];
-    RLcp33_29 = ROcp33_820*dpt[3][36];
-    RLcp33_39 = ROcp33_920*dpt[3][36];
-    POcp33_19 = POcp33_17+RLcp33_19;
-    POcp33_29 = POcp33_27+RLcp33_29;
-    POcp33_39 = POcp33_37+RLcp33_39;
-    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
-    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
-    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
-    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
-    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
-    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
-    VIcp33_19 = ORcp33_19+VIcp33_17;
-    VIcp33_29 = ORcp33_29+VIcp33_27;
-    VIcp33_39 = ORcp33_39+VIcp33_37;
-    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
-    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
-    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
-    POcp33_110 = POcp33_19+RLcp33_110;
-    POcp33_210 = POcp33_29+RLcp33_210;
-    POcp33_310 = POcp33_39+RLcp33_310;
-    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
-    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
-    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
-    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
-    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
-    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
-    VIcp33_110 = ORcp33_110+VIcp33_19;
-    VIcp33_210 = ORcp33_210+VIcp33_29;
-    VIcp33_310 = ORcp33_310+VIcp33_39;
-    RLcp33_111 = ROcp33_421*dpt[2][43];
-    RLcp33_211 = ROcp33_521*dpt[2][43];
-    RLcp33_311 = ROcp33_621*dpt[2][43];
-    POcp33_111 = POcp33_110+RLcp33_111;
-    POcp33_211 = POcp33_210+RLcp33_211;
-    POcp33_311 = POcp33_310+RLcp33_311;
-    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
-    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
-    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
-    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
-    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
-    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
-    VIcp33_111 = ORcp33_111+VIcp33_110;
-    VIcp33_211 = ORcp33_211+VIcp33_210;
-    VIcp33_311 = ORcp33_311+VIcp33_310;
-    RLcp33_112 = ROcp33_723*dpt[3][45];
-    RLcp33_212 = ROcp33_823*dpt[3][45];
-    RLcp33_312 = ROcp33_923*dpt[3][45];
-    POcp33_112 = POcp33_111+RLcp33_112;
-    POcp33_212 = POcp33_211+RLcp33_212;
-    POcp33_312 = POcp33_311+RLcp33_312;
-    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
-    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
-    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
-    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
-    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
-    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
-    VIcp33_112 = ORcp33_112+VIcp33_111;
-    VIcp33_212 = ORcp33_212+VIcp33_211;
-    VIcp33_312 = ORcp33_312+VIcp33_311;
-    PxF9[1] = POcp33_112;
-    PxF9[2] = POcp33_212;
-    PxF9[3] = POcp33_312;
-    RxF9[1][1] = ROcp33_124;
-    RxF9[1][2] = ROcp33_224;
-    RxF9[1][3] = ROcp33_324;
-    RxF9[2][1] = ROcp33_424;
-    RxF9[2][2] = ROcp33_524;
-    RxF9[2][3] = ROcp33_624;
-    RxF9[3][1] = ROcp33_723;
-    RxF9[3][2] = ROcp33_823;
-    RxF9[3][3] = ROcp33_923;
-    VxF9[1] = VIcp33_112;
-    VxF9[2] = VIcp33_212;
-    VxF9[3] = VIcp33_312;
-    OMxF9[1] = OMcp33_112;
-    OMxF9[2] = OMcp33_212;
-    OMxF9[3] = OMcp33_312;
-    ROcp34_25 = S4*S5;
-    ROcp34_35 = -C4*S5;
-    ROcp34_85 = -S4*C5;
-    ROcp34_95 = C4*C5;
-    ROcp34_16 = C5*C6;
-    ROcp34_26 = ROcp34_25*C6+C4*S6;
-    ROcp34_36 = ROcp34_35*C6+S4*S6;
-    ROcp34_46 = -C5*S6;
-    ROcp34_56 = -ROcp34_25*S6+C4*C6;
-    ROcp34_66 = -ROcp34_35*S6+S4*C6;
-    ROcp34_419 = ROcp34_46*C19+S19*S5;
-    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
-    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
-    ROcp34_719 = -ROcp34_46*S19+C19*S5;
-    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
-    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
-    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
-    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
-    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
-    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
-    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
-    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
-    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
-    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
-    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
-    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
-    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
-    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
-    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
-    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
-    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
-    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
-    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
-    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
-    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
-    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
-    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
-    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
-    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
-    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
-    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
-    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
-    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
-    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
-    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
-    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
-    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
-    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
-    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
-    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
-    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
-    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
-    OMcp34_25 = qd[5]*C4;
-    OMcp34_35 = qd[5]*S4;
-    OMcp34_16 = qd[4]+qd[6]*S5;
-    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
-    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
-    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
-    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
-    POcp34_17 = q[1]+RLcp34_17;
-    POcp34_27 = q[2]+RLcp34_27;
-    POcp34_37 = q[3]+RLcp34_37;
-    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
-    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
-    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
-    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
-    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
-    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
-    VIcp34_17 = qd[1]+ORcp34_17;
-    VIcp34_27 = qd[2]+ORcp34_27;
-    VIcp34_37 = qd[3]+ORcp34_37;
-    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
-    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
-    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
-    RLcp34_19 = ROcp34_720*dpt[3][36];
-    RLcp34_29 = ROcp34_820*dpt[3][36];
-    RLcp34_39 = ROcp34_920*dpt[3][36];
-    POcp34_19 = POcp34_17+RLcp34_19;
-    POcp34_29 = POcp34_27+RLcp34_29;
-    POcp34_39 = POcp34_37+RLcp34_39;
-    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
-    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
-    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
-    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
-    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
-    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
-    VIcp34_19 = ORcp34_19+VIcp34_17;
-    VIcp34_29 = ORcp34_29+VIcp34_27;
-    VIcp34_39 = ORcp34_39+VIcp34_37;
-    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
-    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
-    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
-    POcp34_110 = POcp34_19+RLcp34_110;
-    POcp34_210 = POcp34_29+RLcp34_210;
-    POcp34_310 = POcp34_39+RLcp34_310;
-    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
-    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
-    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
-    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
-    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
-    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
-    VIcp34_110 = ORcp34_110+VIcp34_19;
-    VIcp34_210 = ORcp34_210+VIcp34_29;
-    VIcp34_310 = ORcp34_310+VIcp34_39;
-    RLcp34_111 = ROcp34_421*dpt[2][43];
-    RLcp34_211 = ROcp34_521*dpt[2][43];
-    RLcp34_311 = ROcp34_621*dpt[2][43];
-    POcp34_111 = POcp34_110+RLcp34_111;
-    POcp34_211 = POcp34_210+RLcp34_211;
-    POcp34_311 = POcp34_310+RLcp34_311;
-    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
-    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
-    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
-    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
-    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
-    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
-    VIcp34_111 = ORcp34_111+VIcp34_110;
-    VIcp34_211 = ORcp34_211+VIcp34_210;
-    VIcp34_311 = ORcp34_311+VIcp34_310;
-    RLcp34_112 = ROcp34_723*dpt[3][45];
-    RLcp34_212 = ROcp34_823*dpt[3][45];
-    RLcp34_312 = ROcp34_923*dpt[3][45];
-    POcp34_112 = POcp34_111+RLcp34_112;
-    POcp34_212 = POcp34_211+RLcp34_212;
-    POcp34_312 = POcp34_311+RLcp34_312;
-    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
-    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
-    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
-    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
-    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
-    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
-    VIcp34_112 = ORcp34_112+VIcp34_111;
-    VIcp34_212 = ORcp34_212+VIcp34_211;
-    VIcp34_312 = ORcp34_312+VIcp34_311;
-    RLcp34_113 = ROcp34_723*dpt[3][48];
-    RLcp34_213 = ROcp34_823*dpt[3][48];
-    RLcp34_313 = ROcp34_923*dpt[3][48];
-    POcp34_113 = POcp34_112+RLcp34_113;
-    POcp34_213 = POcp34_212+RLcp34_213;
-    POcp34_313 = POcp34_312+RLcp34_313;
-    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
-    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
-    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
-    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
-    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
-    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
-    VIcp34_113 = ORcp34_113+VIcp34_112;
-    VIcp34_213 = ORcp34_213+VIcp34_212;
-    VIcp34_313 = ORcp34_313+VIcp34_312;
-    PxF10[1] = POcp34_113;
-    PxF10[2] = POcp34_213;
-    PxF10[3] = POcp34_313;
-    RxF10[1][1] = ROcp34_125;
-    RxF10[1][2] = ROcp34_225;
-    RxF10[1][3] = ROcp34_325;
-    RxF10[2][1] = ROcp34_424;
-    RxF10[2][2] = ROcp34_524;
-    RxF10[2][3] = ROcp34_624;
-    RxF10[3][1] = ROcp34_725;
-    RxF10[3][2] = ROcp34_825;
-    RxF10[3][3] = ROcp34_925;
-    VxF10[1] = VIcp34_113;
-    VxF10[2] = VIcp34_213;
-    VxF10[3] = VIcp34_313;
-    OMxF10[1] = OMcp34_113;
-    OMxF10[2] = OMcp34_213;
-    OMxF10[3] = OMcp34_313;
-    ROcp35_25 = S4*S5;
-    ROcp35_35 = -C4*S5;
-    ROcp35_85 = -S4*C5;
-    ROcp35_95 = C4*C5;
-    ROcp35_16 = C5*C6;
-    ROcp35_26 = ROcp35_25*C6+C4*S6;
-    ROcp35_36 = ROcp35_35*C6+S4*S6;
-    ROcp35_46 = -C5*S6;
-    ROcp35_56 = -ROcp35_25*S6+C4*C6;
-    ROcp35_66 = -ROcp35_35*S6+S4*C6;
-    ROcp35_419 = ROcp35_46*C19+S19*S5;
-    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
-    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
-    ROcp35_719 = -ROcp35_46*S19+C19*S5;
-    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
-    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
-    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
-    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
-    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
-    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
-    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
-    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
-    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
-    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
-    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
-    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
-    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
-    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
-    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
-    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
-    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
-    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
-    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
-    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
-    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
-    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
-    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
-    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
-    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
-    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
-    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
-    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
-    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
-    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
-    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
-    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
-    OMcp35_25 = qd[5]*C4;
-    OMcp35_35 = qd[5]*S4;
-    OMcp35_16 = qd[4]+qd[6]*S5;
-    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
-    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
-    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
-    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
-    POcp35_17 = q[1]+RLcp35_17;
-    POcp35_27 = q[2]+RLcp35_27;
-    POcp35_37 = q[3]+RLcp35_37;
-    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
-    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
-    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
-    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
-    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
-    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
-    VIcp35_17 = qd[1]+ORcp35_17;
-    VIcp35_27 = qd[2]+ORcp35_27;
-    VIcp35_37 = qd[3]+ORcp35_37;
-    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
-    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
-    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
-    RLcp35_19 = ROcp35_720*dpt[3][36];
-    RLcp35_29 = ROcp35_820*dpt[3][36];
-    RLcp35_39 = ROcp35_920*dpt[3][36];
-    POcp35_19 = POcp35_17+RLcp35_19;
-    POcp35_29 = POcp35_27+RLcp35_29;
-    POcp35_39 = POcp35_37+RLcp35_39;
-    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
-    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
-    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
-    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
-    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
-    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
-    VIcp35_19 = ORcp35_19+VIcp35_17;
-    VIcp35_29 = ORcp35_29+VIcp35_27;
-    VIcp35_39 = ORcp35_39+VIcp35_37;
-    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
-    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
-    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
-    POcp35_110 = POcp35_19+RLcp35_110;
-    POcp35_210 = POcp35_29+RLcp35_210;
-    POcp35_310 = POcp35_39+RLcp35_310;
-    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
-    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
-    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
-    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
-    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
-    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
-    VIcp35_110 = ORcp35_110+VIcp35_19;
-    VIcp35_210 = ORcp35_210+VIcp35_29;
-    VIcp35_310 = ORcp35_310+VIcp35_39;
-    RLcp35_111 = ROcp35_421*dpt[2][52];
-    RLcp35_211 = ROcp35_521*dpt[2][52];
-    RLcp35_311 = ROcp35_621*dpt[2][52];
-    POcp35_111 = POcp35_110+RLcp35_111;
-    POcp35_211 = POcp35_210+RLcp35_211;
-    POcp35_311 = POcp35_310+RLcp35_311;
-    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
-    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
-    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
-    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
-    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
-    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
-    VIcp35_111 = ORcp35_111+VIcp35_110;
-    VIcp35_211 = ORcp35_211+VIcp35_210;
-    VIcp35_311 = ORcp35_311+VIcp35_310;
-    RLcp35_112 = ROcp35_727*dpt[3][54];
-    RLcp35_212 = ROcp35_827*dpt[3][54];
-    RLcp35_312 = ROcp35_927*dpt[3][54];
-    POcp35_112 = POcp35_111+RLcp35_112;
-    POcp35_212 = POcp35_211+RLcp35_212;
-    POcp35_312 = POcp35_311+RLcp35_312;
-    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
-    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
-    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
-    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
-    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
-    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
-    VIcp35_112 = ORcp35_112+VIcp35_111;
-    VIcp35_212 = ORcp35_212+VIcp35_211;
-    VIcp35_312 = ORcp35_312+VIcp35_311;
-    PxF11[1] = POcp35_112;
-    PxF11[2] = POcp35_212;
-    PxF11[3] = POcp35_312;
-    RxF11[1][1] = ROcp35_128;
-    RxF11[1][2] = ROcp35_228;
-    RxF11[1][3] = ROcp35_328;
-    RxF11[2][1] = ROcp35_428;
-    RxF11[2][2] = ROcp35_528;
-    RxF11[2][3] = ROcp35_628;
-    RxF11[3][1] = ROcp35_727;
-    RxF11[3][2] = ROcp35_827;
-    RxF11[3][3] = ROcp35_927;
-    VxF11[1] = VIcp35_112;
-    VxF11[2] = VIcp35_212;
-    VxF11[3] = VIcp35_312;
-    OMxF11[1] = OMcp35_112;
-    OMxF11[2] = OMcp35_212;
-    OMxF11[3] = OMcp35_312;
-    ROcp36_25 = S4*S5;
-    ROcp36_35 = -C4*S5;
-    ROcp36_85 = -S4*C5;
-    ROcp36_95 = C4*C5;
-    ROcp36_16 = C5*C6;
-    ROcp36_26 = ROcp36_25*C6+C4*S6;
-    ROcp36_36 = ROcp36_35*C6+S4*S6;
-    ROcp36_46 = -C5*S6;
-    ROcp36_56 = -ROcp36_25*S6+C4*C6;
-    ROcp36_66 = -ROcp36_35*S6+S4*C6;
-    ROcp36_419 = ROcp36_46*C19+S19*S5;
-    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
-    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
-    ROcp36_719 = -ROcp36_46*S19+C19*S5;
-    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
-    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
-    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
-    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
-    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
-    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
-    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
-    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
-    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
-    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
-    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
-    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
-    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
-    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
-    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
-    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
-    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
-    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
-    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
-    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
-    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
-    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
-    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
-    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
-    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
-    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
-    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
-    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
-    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
-    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
-    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
-    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
-    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
-    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
-    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
-    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
-    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
-    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
-    OMcp36_25 = qd[5]*C4;
-    OMcp36_35 = qd[5]*S4;
-    OMcp36_16 = qd[4]+qd[6]*S5;
-    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
-    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
-    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
-    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
-    POcp36_17 = q[1]+RLcp36_17;
-    POcp36_27 = q[2]+RLcp36_27;
-    POcp36_37 = q[3]+RLcp36_37;
-    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
-    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
-    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
-    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
-    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
-    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
-    VIcp36_17 = qd[1]+ORcp36_17;
-    VIcp36_27 = qd[2]+ORcp36_27;
-    VIcp36_37 = qd[3]+ORcp36_37;
-    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
-    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
-    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
-    RLcp36_19 = ROcp36_720*dpt[3][36];
-    RLcp36_29 = ROcp36_820*dpt[3][36];
-    RLcp36_39 = ROcp36_920*dpt[3][36];
-    POcp36_19 = POcp36_17+RLcp36_19;
-    POcp36_29 = POcp36_27+RLcp36_29;
-    POcp36_39 = POcp36_37+RLcp36_39;
-    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
-    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
-    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
-    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
-    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
-    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
-    VIcp36_19 = ORcp36_19+VIcp36_17;
-    VIcp36_29 = ORcp36_29+VIcp36_27;
-    VIcp36_39 = ORcp36_39+VIcp36_37;
-    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
-    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
-    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
-    POcp36_110 = POcp36_19+RLcp36_110;
-    POcp36_210 = POcp36_29+RLcp36_210;
-    POcp36_310 = POcp36_39+RLcp36_310;
-    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
-    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
-    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
-    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
-    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
-    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
-    VIcp36_110 = ORcp36_110+VIcp36_19;
-    VIcp36_210 = ORcp36_210+VIcp36_29;
-    VIcp36_310 = ORcp36_310+VIcp36_39;
-    RLcp36_111 = ROcp36_421*dpt[2][52];
-    RLcp36_211 = ROcp36_521*dpt[2][52];
-    RLcp36_311 = ROcp36_621*dpt[2][52];
-    POcp36_111 = POcp36_110+RLcp36_111;
-    POcp36_211 = POcp36_210+RLcp36_211;
-    POcp36_311 = POcp36_310+RLcp36_311;
-    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
-    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
-    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
-    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
-    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
-    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
-    VIcp36_111 = ORcp36_111+VIcp36_110;
-    VIcp36_211 = ORcp36_211+VIcp36_210;
-    VIcp36_311 = ORcp36_311+VIcp36_310;
-    RLcp36_112 = ROcp36_727*dpt[3][54];
-    RLcp36_212 = ROcp36_827*dpt[3][54];
-    RLcp36_312 = ROcp36_927*dpt[3][54];
-    POcp36_112 = POcp36_111+RLcp36_112;
-    POcp36_212 = POcp36_211+RLcp36_212;
-    POcp36_312 = POcp36_311+RLcp36_312;
-    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
-    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
-    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
-    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
-    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
-    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
-    VIcp36_112 = ORcp36_112+VIcp36_111;
-    VIcp36_212 = ORcp36_212+VIcp36_211;
-    VIcp36_312 = ORcp36_312+VIcp36_311;
-    RLcp36_113 = ROcp36_727*dpt[3][57];
-    RLcp36_213 = ROcp36_827*dpt[3][57];
-    RLcp36_313 = ROcp36_927*dpt[3][57];
-    POcp36_113 = POcp36_112+RLcp36_113;
-    POcp36_213 = POcp36_212+RLcp36_213;
-    POcp36_313 = POcp36_312+RLcp36_313;
-    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
-    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
-    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
-    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
-    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
-    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
-    VIcp36_113 = ORcp36_113+VIcp36_112;
-    VIcp36_213 = ORcp36_213+VIcp36_212;
-    VIcp36_313 = ORcp36_313+VIcp36_312;
-    PxF12[1] = POcp36_113;
-    PxF12[2] = POcp36_213;
-    PxF12[3] = POcp36_313;
-    RxF12[1][1] = ROcp36_129;
-    RxF12[1][2] = ROcp36_229;
-    RxF12[1][3] = ROcp36_329;
-    RxF12[2][1] = ROcp36_428;
-    RxF12[2][2] = ROcp36_528;
-    RxF12[2][3] = ROcp36_628;
-    RxF12[3][1] = ROcp36_729;
-    RxF12[3][2] = ROcp36_829;
-    RxF12[3][3] = ROcp36_929;
-    VxF12[1] = VIcp36_113;
-    VxF12[2] = VIcp36_213;
-    VxF12[3] = VIcp36_313;
-    OMxF12[1] = OMcp36_113;
-    OMxF12[2] = OMcp36_213;
-    OMxF12[3] = OMcp36_313;
- 
-    // output copy
-    for(i=1; i<=3; i++)
-    {
-        PxF_tab[0][i] = PxF1[i];
-        PxF_tab[1][i] = PxF2[i];
-        PxF_tab[2][i] = PxF3[i];
-        PxF_tab[3][i] = PxF4[i];
-        PxF_tab[4][i] = PxF5[i];
-        PxF_tab[5][i] = PxF6[i];
-        PxF_tab[6][i] = PxF7[i];
-        PxF_tab[7][i] = PxF8[i];
-        PxF_tab[8][i] = PxF9[i];
-        PxF_tab[9][i] = PxF10[i];
-        PxF_tab[10][i] = PxF11[i];
-        PxF_tab[11][i] = PxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        for(j=1; j<=3; j++)
-        {
-        RxF_tab[0][i][j] = RxF1[i][j];
-        RxF_tab[1][i][j] = RxF2[i][j];
-        RxF_tab[2][i][j] = RxF3[i][j];
-        RxF_tab[3][i][j] = RxF4[i][j];
-        RxF_tab[4][i][j] = RxF5[i][j];
-        RxF_tab[5][i][j] = RxF6[i][j];
-        RxF_tab[6][i][j] = RxF7[i][j];
-        RxF_tab[7][i][j] = RxF8[i][j];
-        RxF_tab[8][i][j] = RxF9[i][j];
-        RxF_tab[9][i][j] = RxF10[i][j];
-        RxF_tab[10][i][j] = RxF11[i][j];
-        RxF_tab[11][i][j] = RxF12[i][j];
-        }
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        VxF_tab[0][i] = VxF1[i];
-        VxF_tab[1][i] = VxF2[i];
-        VxF_tab[2][i] = VxF3[i];
-        VxF_tab[3][i] = VxF4[i];
-        VxF_tab[4][i] = VxF5[i];
-        VxF_tab[5][i] = VxF6[i];
-        VxF_tab[6][i] = VxF7[i];
-        VxF_tab[7][i] = VxF8[i];
-        VxF_tab[8][i] = VxF9[i];
-        VxF_tab[9][i] = VxF10[i];
-        VxF_tab[10][i] = VxF11[i];
-        VxF_tab[11][i] = VxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        OMxF_tab[0][i] = OMxF1[i];
-        OMxF_tab[1][i] = OMxF2[i];
-        OMxF_tab[2][i] = OMxF3[i];
-        OMxF_tab[3][i] = OMxF4[i];
-        OMxF_tab[4][i] = OMxF5[i];
-        OMxF_tab[5][i] = OMxF6[i];
-        OMxF_tab[6][i] = OMxF7[i];
-        OMxF_tab[7][i] = OMxF8[i];
-        OMxF_tab[8][i] = OMxF9[i];
-        OMxF_tab[9][i] = OMxF10[i];
-        OMxF_tab[10][i] = OMxF11[i];
-        OMxF_tab[11][i] = OMxF12[i];
-    }
-
-}
diff --git a/symbolicR/short_feet/CMakeLists.txt b/symbolicR/short_feet/CMakeLists.txt
index 3bae37b..c963c74 100644
--- a/symbolicR/short_feet/CMakeLists.txt
+++ b/symbolicR/short_feet/CMakeLists.txt
@@ -1,12 +1,22 @@
-# Copyright: (C)
-# Authors: Timothee Habra & Nicolas Van der Noot
-# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# CMake for compiling the symbolic files of a robotran project in C
+# 
 
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 #                       LIBRARY MAIN CONFIGURATIONS
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
-cmake_minimum_required(VERSION 2.8.7)
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
 
 project(Project_symbolic)
 
@@ -49,10 +59,26 @@ include_directories ( ${INCLUDE_DIR} )
 include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
 
 if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
+
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
     add_library(Project_symbolic SHARED ${SOURCE_FILES})
+	
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_symbolic PROPERTIES SUFFIX .so)    
+    endif()    
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
+        add_custom_command(TARGET Project_symbolic POST_BUILD
+			COMMAND ${CMAKE_COMMAND} -E copy
+                        ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_symbolic.dll
+                        ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_symbolic.dll
+        )
+    endif()
+
 else()
     add_library(Project_symbolic STATIC ${SOURCE_FILES})
-    add_definitions( -DPROJECT_USERFCT_STATIC_DEFINE)
 
     # some symbolic routines call user functions (ext_forces, accelred, ...)
     target_link_libraries(Project_symbolic Project_userfct)
@@ -67,14 +93,5 @@ endif()
 if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
     find_package( LibRobotranC REQUIRED )
     add_definitions(${LIB_MBSYSC_DEFINITIONS})
+    target_link_libraries(Project_symbolic ${LIB_MBSYSC_MODULES})
 endif()
-
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         STUFF FOR WINDOWS (TO BE CLEANED)
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-include (GenerateExportHeader)
-GENERATE_EXPORT_HEADER( Project_symbolic    )
-
-install (TARGETS Project_symbolic DESTINATION ${CMAKE_INSTALL_PREFIX})
diff --git a/symbolicR/short_feet/user_kinematics_model.c b/symbolicR/short_feet/user_kinematics_model.c
deleted file mode 100644
index 4d78acd..0000000
--- a/symbolicR/short_feet/user_kinematics_model.c
+++ /dev/null
@@ -1,1733 +0,0 @@
-#include "mbs_data.h"
-
-#include "math.h"
-
-/*! \brief compute the kinematics model of sensors with ExtForces
- * 
- * \param[in] mbs_data Robotran structure
- * \param[out] PxF_tab tabular with all the 'PxF' computed
- * \param[out] RxF_tab tabular with all the 'RxF' computed
- * \param[out] VxF_tab tabular with all the 'VxF' computed
- * \param[out] OMxF_tab tabular with all the 'OMxF' computed
- */
-void user_kinematics_model(MbsData *mbs_data, double **PxF_tab, double ***RxF_tab, double **VxF_tab, double **OMxF_tab)
-{
-    // variables declaration
-    int i, j;
-    double *q, *qd;
-    double **dpt;
-
-    double PxF1[4], PxF2[4], PxF3[4], PxF4[4], PxF5[4], PxF6[4], PxF7[4], PxF8[4], PxF9[4], PxF10[4], PxF11[4], PxF12[4];
-    double RxF1[4][4], RxF2[4][4], RxF3[4][4], RxF4[4][4], RxF5[4][4], RxF6[4][4], RxF7[4][4], RxF8[4][4], RxF9[4][4], RxF10[4][4], RxF11[4][4], RxF12[4][4];
-    double VxF1[4], VxF2[4], VxF3[4], VxF4[4], VxF5[4], VxF6[4], VxF7[4], VxF8[4], VxF9[4], VxF10[4], VxF11[4], VxF12[4];
-    double OMxF1[4], OMxF2[4], OMxF3[4], OMxF4[4], OMxF5[4], OMxF6[4], OMxF7[4], OMxF8[4], OMxF9[4], OMxF10[4], OMxF11[4], OMxF12[4];
-
-    double C4, C5, C6, C7, C8, C9, C10, C11, C12, C13, C14, C15, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26, C27, C28, C29;
-    double S4, S5, S6, S7, S8, S9, S10, S11, S12, S13, S14, S15, S16, S17, S18, S19, S20, S21, S22, S23, S24, S25, S26, S27, S28, S29;
-
-    double ROcp25_25, ROcp25_35, ROcp25_85, ROcp25_95, ROcp25_16, ROcp25_26, ROcp25_36, ROcp25_46, ROcp25_56, ROcp25_66, OMcp25_25, OMcp25_35, OMcp25_16, OMcp25_26, OMcp25_36;
-    double ROcp26_25, ROcp26_35, ROcp26_85, ROcp26_95, ROcp26_16, ROcp26_26, ROcp26_36, ROcp26_46, ROcp26_56, ROcp26_66, ROcp26_17, ROcp26_27, ROcp26_37, ROcp26_77, ROcp26_87;
-    double ROcp26_97, ROcp26_48, ROcp26_58, ROcp26_68, ROcp26_78, ROcp26_88, ROcp26_98, ROcp26_19, ROcp26_29, ROcp26_39, ROcp26_49, ROcp26_59, ROcp26_69, OMcp26_25, OMcp26_35;
-    double OMcp26_16, OMcp26_26, OMcp26_36, RLcp26_17, RLcp26_27, RLcp26_37, POcp26_17, POcp26_27, POcp26_37, OMcp26_17, OMcp26_27, OMcp26_37, ORcp26_17, ORcp26_27, ORcp26_37;
-    double VIcp26_17, VIcp26_27, VIcp26_37, RLcp26_18, RLcp26_28, RLcp26_38, POcp26_18, POcp26_28, POcp26_38, OMcp26_18, OMcp26_28, OMcp26_38, ORcp26_18, ORcp26_28, ORcp26_38;
-    double VIcp26_18, VIcp26_28, VIcp26_38, RLcp26_19, RLcp26_29, RLcp26_39, POcp26_19, POcp26_29, POcp26_39, OMcp26_19, OMcp26_29, OMcp26_39, ORcp26_19, ORcp26_29, ORcp26_39;
-    double VIcp26_19, VIcp26_29, VIcp26_39, ROcp27_25, ROcp27_35, ROcp27_85, ROcp27_95, ROcp27_16, ROcp27_26, ROcp27_36, ROcp27_46, ROcp27_56, ROcp27_66, ROcp27_17, ROcp27_27;
-    double ROcp27_37, ROcp27_77, ROcp27_87, ROcp27_97, ROcp27_48, ROcp27_58, ROcp27_68, ROcp27_78, ROcp27_88, ROcp27_98, ROcp27_19, ROcp27_29, ROcp27_39, ROcp27_49, ROcp27_59;
-    double ROcp27_69, ROcp27_110, ROcp27_210, ROcp27_310, ROcp27_710, ROcp27_810, ROcp27_910, OMcp27_25, OMcp27_35, OMcp27_16, OMcp27_26, OMcp27_36, RLcp27_17, RLcp27_27, RLcp27_37;
-    double POcp27_17, POcp27_27, POcp27_37, OMcp27_17, OMcp27_27, OMcp27_37, ORcp27_17, ORcp27_27, ORcp27_37, VIcp27_17, VIcp27_27, VIcp27_37, RLcp27_18, RLcp27_28, RLcp27_38;
-    double POcp27_18, POcp27_28, POcp27_38, OMcp27_18, OMcp27_28, OMcp27_38, ORcp27_18, ORcp27_28, ORcp27_38, VIcp27_18, VIcp27_28, VIcp27_38, RLcp27_19, RLcp27_29, RLcp27_39;
-    double POcp27_19, POcp27_29, POcp27_39, OMcp27_19, OMcp27_29, OMcp27_39, ORcp27_19, ORcp27_29, ORcp27_39, VIcp27_19, VIcp27_29, VIcp27_39, RLcp27_110, RLcp27_210, RLcp27_310;
-    double POcp27_110, POcp27_210, POcp27_310, OMcp27_110, OMcp27_210, OMcp27_310, ORcp27_110, ORcp27_210, ORcp27_310, VIcp27_110, VIcp27_210, VIcp27_310, ROcp28_25, ROcp28_35, ROcp28_85;
-    double ROcp28_95, ROcp28_16, ROcp28_26, ROcp28_36, ROcp28_46, ROcp28_56, ROcp28_66, ROcp28_17, ROcp28_27, ROcp28_37, ROcp28_77, ROcp28_87, ROcp28_97, ROcp28_48, ROcp28_58;
-    double ROcp28_68, ROcp28_78, ROcp28_88, ROcp28_98, ROcp28_19, ROcp28_29, ROcp28_39, ROcp28_49, ROcp28_59, ROcp28_69, ROcp28_110, ROcp28_210, ROcp28_310, ROcp28_710, ROcp28_810;
-    double ROcp28_910, ROcp28_411, ROcp28_511, ROcp28_611, ROcp28_711, ROcp28_811, ROcp28_911, ROcp28_112, ROcp28_212, ROcp28_312, ROcp28_712, ROcp28_812, ROcp28_912, OMcp28_25, OMcp28_35;
-    double OMcp28_16, OMcp28_26, OMcp28_36, RLcp28_17, RLcp28_27, RLcp28_37, POcp28_17, POcp28_27, POcp28_37, OMcp28_17, OMcp28_27, OMcp28_37, ORcp28_17, ORcp28_27, ORcp28_37;
-    double VIcp28_17, VIcp28_27, VIcp28_37, RLcp28_18, RLcp28_28, RLcp28_38, POcp28_18, POcp28_28, POcp28_38, OMcp28_18, OMcp28_28, OMcp28_38, ORcp28_18, ORcp28_28, ORcp28_38;
-    double VIcp28_18, VIcp28_28, VIcp28_38, RLcp28_19, RLcp28_29, RLcp28_39, POcp28_19, POcp28_29, POcp28_39, OMcp28_19, OMcp28_29, OMcp28_39, ORcp28_19, ORcp28_29, ORcp28_39;
-    double VIcp28_19, VIcp28_29, VIcp28_39, RLcp28_110, RLcp28_210, RLcp28_310, POcp28_110, POcp28_210, POcp28_310, OMcp28_110, OMcp28_210, OMcp28_310, ORcp28_110, ORcp28_210, ORcp28_310;
-    double VIcp28_110, VIcp28_210, VIcp28_310, RLcp28_111, RLcp28_211, RLcp28_311, POcp28_111, POcp28_211, POcp28_311, OMcp28_111, OMcp28_211, OMcp28_311, ORcp28_111, ORcp28_211, ORcp28_311;
-    double VIcp28_111, VIcp28_211, VIcp28_311, OMcp28_112, OMcp28_212, OMcp28_312, RLcp28_113, RLcp28_213, RLcp28_313, POcp28_113, POcp28_213, POcp28_313, ORcp28_113, ORcp28_213, ORcp28_313;
-    double VIcp28_113, VIcp28_213, VIcp28_313, ROcp29_25, ROcp29_35, ROcp29_85, ROcp29_95, ROcp29_16, ROcp29_26, ROcp29_36, ROcp29_46, ROcp29_56, ROcp29_66, ROcp29_113, ROcp29_213;
-    double ROcp29_313, ROcp29_713, ROcp29_813, ROcp29_913, ROcp29_414, ROcp29_514, ROcp29_614, ROcp29_714, ROcp29_814, ROcp29_914, ROcp29_115, ROcp29_215, ROcp29_315, ROcp29_415, ROcp29_515;
-    double ROcp29_615, OMcp29_25, OMcp29_35, OMcp29_16, OMcp29_26, OMcp29_36, RLcp29_17, RLcp29_27, RLcp29_37, POcp29_17, POcp29_27, POcp29_37, OMcp29_17, OMcp29_27, OMcp29_37;
-    double ORcp29_17, ORcp29_27, ORcp29_37, VIcp29_17, VIcp29_27, VIcp29_37, RLcp29_18, RLcp29_28, RLcp29_38, POcp29_18, POcp29_28, POcp29_38, OMcp29_18, OMcp29_28, OMcp29_38;
-    double ORcp29_18, ORcp29_28, ORcp29_38, VIcp29_18, VIcp29_28, VIcp29_38, RLcp29_19, RLcp29_29, RLcp29_39, POcp29_19, POcp29_29, POcp29_39, OMcp29_19, OMcp29_29, OMcp29_39;
-    double ORcp29_19, ORcp29_29, ORcp29_39, VIcp29_19, VIcp29_29, VIcp29_39, ROcp30_25, ROcp30_35, ROcp30_85, ROcp30_95, ROcp30_16, ROcp30_26, ROcp30_36, ROcp30_46, ROcp30_56;
-    double ROcp30_66, ROcp30_113, ROcp30_213, ROcp30_313, ROcp30_713, ROcp30_813, ROcp30_913, ROcp30_414, ROcp30_514, ROcp30_614, ROcp30_714, ROcp30_814, ROcp30_914, ROcp30_115, ROcp30_215;
-    double ROcp30_315, ROcp30_415, ROcp30_515, ROcp30_615, ROcp30_116, ROcp30_216, ROcp30_316, ROcp30_716, ROcp30_816, ROcp30_916, OMcp30_25, OMcp30_35, OMcp30_16, OMcp30_26, OMcp30_36;
-    double RLcp30_17, RLcp30_27, RLcp30_37, POcp30_17, POcp30_27, POcp30_37, OMcp30_17, OMcp30_27, OMcp30_37, ORcp30_17, ORcp30_27, ORcp30_37, VIcp30_17, VIcp30_27, VIcp30_37;
-    double RLcp30_18, RLcp30_28, RLcp30_38, POcp30_18, POcp30_28, POcp30_38, OMcp30_18, OMcp30_28, OMcp30_38, ORcp30_18, ORcp30_28, ORcp30_38, VIcp30_18, VIcp30_28, VIcp30_38;
-    double RLcp30_19, RLcp30_29, RLcp30_39, POcp30_19, POcp30_29, POcp30_39, OMcp30_19, OMcp30_29, OMcp30_39, ORcp30_19, ORcp30_29, ORcp30_39, VIcp30_19, VIcp30_29, VIcp30_39;
-    double RLcp30_110, RLcp30_210, RLcp30_310, POcp30_110, POcp30_210, POcp30_310, OMcp30_110, OMcp30_210, OMcp30_310, ORcp30_110, ORcp30_210, ORcp30_310, VIcp30_110, VIcp30_210, VIcp30_310;
-    double ROcp31_25, ROcp31_35, ROcp31_85, ROcp31_95, ROcp31_16, ROcp31_26, ROcp31_36, ROcp31_46, ROcp31_56, ROcp31_66, ROcp31_113, ROcp31_213, ROcp31_313, ROcp31_713, ROcp31_813;
-    double ROcp31_913, ROcp31_414, ROcp31_514, ROcp31_614, ROcp31_714, ROcp31_814, ROcp31_914, ROcp31_115, ROcp31_215, ROcp31_315, ROcp31_415, ROcp31_515, ROcp31_615, ROcp31_116, ROcp31_216;
-    double ROcp31_316, ROcp31_716, ROcp31_816, ROcp31_916, ROcp31_417, ROcp31_517, ROcp31_617, ROcp31_717, ROcp31_817, ROcp31_917, ROcp31_118, ROcp31_218, ROcp31_318, ROcp31_718, ROcp31_818;
-    double ROcp31_918, OMcp31_25, OMcp31_35, OMcp31_16, OMcp31_26, OMcp31_36, RLcp31_17, RLcp31_27, RLcp31_37, POcp31_17, POcp31_27, POcp31_37, OMcp31_17, OMcp31_27, OMcp31_37;
-    double ORcp31_17, ORcp31_27, ORcp31_37, VIcp31_17, VIcp31_27, VIcp31_37, RLcp31_18, RLcp31_28, RLcp31_38, POcp31_18, POcp31_28, POcp31_38, OMcp31_18, OMcp31_28, OMcp31_38;
-    double ORcp31_18, ORcp31_28, ORcp31_38, VIcp31_18, VIcp31_28, VIcp31_38, RLcp31_19, RLcp31_29, RLcp31_39, POcp31_19, POcp31_29, POcp31_39, OMcp31_19, OMcp31_29, OMcp31_39;
-    double ORcp31_19, ORcp31_29, ORcp31_39, VIcp31_19, VIcp31_29, VIcp31_39, RLcp31_110, RLcp31_210, RLcp31_310, POcp31_110, POcp31_210, POcp31_310, OMcp31_110, OMcp31_210, OMcp31_310;
-    double ORcp31_110, ORcp31_210, ORcp31_310, VIcp31_110, VIcp31_210, VIcp31_310, RLcp31_111, RLcp31_211, RLcp31_311, POcp31_111, POcp31_211, POcp31_311, OMcp31_111, OMcp31_211, OMcp31_311;
-    double ORcp31_111, ORcp31_211, ORcp31_311, VIcp31_111, VIcp31_211, VIcp31_311, OMcp31_112, OMcp31_212, OMcp31_312, RLcp31_113, RLcp31_213, RLcp31_313, POcp31_113, POcp31_213, POcp31_313;
-    double ORcp31_113, ORcp31_213, ORcp31_313, VIcp31_113, VIcp31_213, VIcp31_313, ROcp32_25, ROcp32_35, ROcp32_85, ROcp32_95, ROcp32_16, ROcp32_26, ROcp32_36, ROcp32_46, ROcp32_56;
-    double ROcp32_66, ROcp32_419, ROcp32_519, ROcp32_619, ROcp32_719, ROcp32_819, ROcp32_919, ROcp32_120, ROcp32_220, ROcp32_320, ROcp32_720, ROcp32_820, ROcp32_920, ROcp32_121, ROcp32_221;
-    double ROcp32_321, ROcp32_421, ROcp32_521, ROcp32_621, OMcp32_25, OMcp32_35, OMcp32_16, OMcp32_26, OMcp32_36, RLcp32_17, RLcp32_27, RLcp32_37, POcp32_17, POcp32_27, POcp32_37;
-    double OMcp32_17, OMcp32_27, OMcp32_37, ORcp32_17, ORcp32_27, ORcp32_37, VIcp32_17, VIcp32_27, VIcp32_37, OMcp32_18, OMcp32_28, OMcp32_38, RLcp32_19, RLcp32_29, RLcp32_39;
-    double POcp32_19, POcp32_29, POcp32_39, OMcp32_19, OMcp32_29, OMcp32_39, ORcp32_19, ORcp32_29, ORcp32_39, VIcp32_19, VIcp32_29, VIcp32_39, ROcp33_25, ROcp33_35, ROcp33_85;
-    double ROcp33_95, ROcp33_16, ROcp33_26, ROcp33_36, ROcp33_46, ROcp33_56, ROcp33_66, ROcp33_419, ROcp33_519, ROcp33_619, ROcp33_719, ROcp33_819, ROcp33_919, ROcp33_120, ROcp33_220;
-    double ROcp33_320, ROcp33_720, ROcp33_820, ROcp33_920, ROcp33_121, ROcp33_221, ROcp33_321, ROcp33_421, ROcp33_521, ROcp33_621, ROcp33_122, ROcp33_222, ROcp33_322, ROcp33_722, ROcp33_822;
-    double ROcp33_922, ROcp33_423, ROcp33_523, ROcp33_623, ROcp33_723, ROcp33_823, ROcp33_923, ROcp33_124, ROcp33_224, ROcp33_324, ROcp33_424, ROcp33_524, ROcp33_624, OMcp33_25, OMcp33_35;
-    double OMcp33_16, OMcp33_26, OMcp33_36, RLcp33_17, RLcp33_27, RLcp33_37, POcp33_17, POcp33_27, POcp33_37, OMcp33_17, OMcp33_27, OMcp33_37, ORcp33_17, ORcp33_27, ORcp33_37;
-    double VIcp33_17, VIcp33_27, VIcp33_37, OMcp33_18, OMcp33_28, OMcp33_38, RLcp33_19, RLcp33_29, RLcp33_39, POcp33_19, POcp33_29, POcp33_39, OMcp33_19, OMcp33_29, OMcp33_39;
-    double ORcp33_19, ORcp33_29, ORcp33_39, VIcp33_19, VIcp33_29, VIcp33_39, RLcp33_110, RLcp33_210, RLcp33_310, POcp33_110, POcp33_210, POcp33_310, OMcp33_110, OMcp33_210, OMcp33_310;
-    double ORcp33_110, ORcp33_210, ORcp33_310, VIcp33_110, VIcp33_210, VIcp33_310, RLcp33_111, RLcp33_211, RLcp33_311, POcp33_111, POcp33_211, POcp33_311, OMcp33_111, OMcp33_211, OMcp33_311;
-    double ORcp33_111, ORcp33_211, ORcp33_311, VIcp33_111, VIcp33_211, VIcp33_311, RLcp33_112, RLcp33_212, RLcp33_312, POcp33_112, POcp33_212, POcp33_312, OMcp33_112, OMcp33_212, OMcp33_312;
-    double ORcp33_112, ORcp33_212, ORcp33_312, VIcp33_112, VIcp33_212, VIcp33_312, ROcp34_25, ROcp34_35, ROcp34_85, ROcp34_95, ROcp34_16, ROcp34_26, ROcp34_36, ROcp34_46, ROcp34_56;
-    double ROcp34_66, ROcp34_419, ROcp34_519, ROcp34_619, ROcp34_719, ROcp34_819, ROcp34_919, ROcp34_120, ROcp34_220, ROcp34_320, ROcp34_720, ROcp34_820, ROcp34_920, ROcp34_121, ROcp34_221;
-    double ROcp34_321, ROcp34_421, ROcp34_521, ROcp34_621, ROcp34_122, ROcp34_222, ROcp34_322, ROcp34_722, ROcp34_822, ROcp34_922, ROcp34_423, ROcp34_523, ROcp34_623, ROcp34_723, ROcp34_823;
-    double ROcp34_923, ROcp34_124, ROcp34_224, ROcp34_324, ROcp34_424, ROcp34_524, ROcp34_624, ROcp34_125, ROcp34_225, ROcp34_325, ROcp34_725, ROcp34_825, ROcp34_925, OMcp34_25, OMcp34_35;
-    double OMcp34_16, OMcp34_26, OMcp34_36, RLcp34_17, RLcp34_27, RLcp34_37, POcp34_17, POcp34_27, POcp34_37, OMcp34_17, OMcp34_27, OMcp34_37, ORcp34_17, ORcp34_27, ORcp34_37;
-    double VIcp34_17, VIcp34_27, VIcp34_37, OMcp34_18, OMcp34_28, OMcp34_38, RLcp34_19, RLcp34_29, RLcp34_39, POcp34_19, POcp34_29, POcp34_39, OMcp34_19, OMcp34_29, OMcp34_39;
-    double ORcp34_19, ORcp34_29, ORcp34_39, VIcp34_19, VIcp34_29, VIcp34_39, RLcp34_110, RLcp34_210, RLcp34_310, POcp34_110, POcp34_210, POcp34_310, OMcp34_110, OMcp34_210, OMcp34_310;
-    double ORcp34_110, ORcp34_210, ORcp34_310, VIcp34_110, VIcp34_210, VIcp34_310, RLcp34_111, RLcp34_211, RLcp34_311, POcp34_111, POcp34_211, POcp34_311, OMcp34_111, OMcp34_211, OMcp34_311;
-    double ORcp34_111, ORcp34_211, ORcp34_311, VIcp34_111, VIcp34_211, VIcp34_311, RLcp34_112, RLcp34_212, RLcp34_312, POcp34_112, POcp34_212, POcp34_312, OMcp34_112, OMcp34_212, OMcp34_312;
-    double ORcp34_112, ORcp34_212, ORcp34_312, VIcp34_112, VIcp34_212, VIcp34_312, RLcp34_113, RLcp34_213, RLcp34_313, POcp34_113, POcp34_213, POcp34_313, OMcp34_113, OMcp34_213, OMcp34_313;
-    double ORcp34_113, ORcp34_213, ORcp34_313, VIcp34_113, VIcp34_213, VIcp34_313, ROcp35_25, ROcp35_35, ROcp35_85, ROcp35_95, ROcp35_16, ROcp35_26, ROcp35_36, ROcp35_46, ROcp35_56;
-    double ROcp35_66, ROcp35_419, ROcp35_519, ROcp35_619, ROcp35_719, ROcp35_819, ROcp35_919, ROcp35_120, ROcp35_220, ROcp35_320, ROcp35_720, ROcp35_820, ROcp35_920, ROcp35_121, ROcp35_221;
-    double ROcp35_321, ROcp35_421, ROcp35_521, ROcp35_621, ROcp35_126, ROcp35_226, ROcp35_326, ROcp35_726, ROcp35_826, ROcp35_926, ROcp35_427, ROcp35_527, ROcp35_627, ROcp35_727, ROcp35_827;
-    double ROcp35_927, ROcp35_128, ROcp35_228, ROcp35_328, ROcp35_428, ROcp35_528, ROcp35_628, OMcp35_25, OMcp35_35, OMcp35_16, OMcp35_26, OMcp35_36, RLcp35_17, RLcp35_27, RLcp35_37;
-    double POcp35_17, POcp35_27, POcp35_37, OMcp35_17, OMcp35_27, OMcp35_37, ORcp35_17, ORcp35_27, ORcp35_37, VIcp35_17, VIcp35_27, VIcp35_37, OMcp35_18, OMcp35_28, OMcp35_38;
-    double RLcp35_19, RLcp35_29, RLcp35_39, POcp35_19, POcp35_29, POcp35_39, OMcp35_19, OMcp35_29, OMcp35_39, ORcp35_19, ORcp35_29, ORcp35_39, VIcp35_19, VIcp35_29, VIcp35_39;
-    double RLcp35_110, RLcp35_210, RLcp35_310, POcp35_110, POcp35_210, POcp35_310, OMcp35_110, OMcp35_210, OMcp35_310, ORcp35_110, ORcp35_210, ORcp35_310, VIcp35_110, VIcp35_210, VIcp35_310;
-    double RLcp35_111, RLcp35_211, RLcp35_311, POcp35_111, POcp35_211, POcp35_311, OMcp35_111, OMcp35_211, OMcp35_311, ORcp35_111, ORcp35_211, ORcp35_311, VIcp35_111, VIcp35_211, VIcp35_311;
-    double RLcp35_112, RLcp35_212, RLcp35_312, POcp35_112, POcp35_212, POcp35_312, OMcp35_112, OMcp35_212, OMcp35_312, ORcp35_112, ORcp35_212, ORcp35_312, VIcp35_112, VIcp35_212, VIcp35_312;
-    double ROcp36_25, ROcp36_35, ROcp36_85, ROcp36_95, ROcp36_16, ROcp36_26, ROcp36_36, ROcp36_46, ROcp36_56, ROcp36_66, ROcp36_419, ROcp36_519, ROcp36_619, ROcp36_719, ROcp36_819;
-    double ROcp36_919, ROcp36_120, ROcp36_220, ROcp36_320, ROcp36_720, ROcp36_820, ROcp36_920, ROcp36_121, ROcp36_221, ROcp36_321, ROcp36_421, ROcp36_521, ROcp36_621, ROcp36_126, ROcp36_226;
-    double ROcp36_326, ROcp36_726, ROcp36_826, ROcp36_926, ROcp36_427, ROcp36_527, ROcp36_627, ROcp36_727, ROcp36_827, ROcp36_927, ROcp36_128, ROcp36_228, ROcp36_328, ROcp36_428, ROcp36_528;
-    double ROcp36_628, ROcp36_129, ROcp36_229, ROcp36_329, ROcp36_729, ROcp36_829, ROcp36_929, OMcp36_25, OMcp36_35, OMcp36_16, OMcp36_26, OMcp36_36, RLcp36_17, RLcp36_27, RLcp36_37;
-    double POcp36_17, POcp36_27, POcp36_37, OMcp36_17, OMcp36_27, OMcp36_37, ORcp36_17, ORcp36_27, ORcp36_37, VIcp36_17, VIcp36_27, VIcp36_37, OMcp36_18, OMcp36_28, OMcp36_38;
-    double RLcp36_19, RLcp36_29, RLcp36_39, POcp36_19, POcp36_29, POcp36_39, OMcp36_19, OMcp36_29, OMcp36_39, ORcp36_19, ORcp36_29, ORcp36_39, VIcp36_19, VIcp36_29, VIcp36_39;
-    double RLcp36_110, RLcp36_210, RLcp36_310, POcp36_110, POcp36_210, POcp36_310, OMcp36_110, OMcp36_210, OMcp36_310, ORcp36_110, ORcp36_210, ORcp36_310, VIcp36_110, VIcp36_210, VIcp36_310;
-    double RLcp36_111, RLcp36_211, RLcp36_311, POcp36_111, POcp36_211, POcp36_311, OMcp36_111, OMcp36_211, OMcp36_311, ORcp36_111, ORcp36_211, ORcp36_311, VIcp36_111, VIcp36_211, VIcp36_311;
-    double RLcp36_112, RLcp36_212, RLcp36_312, POcp36_112, POcp36_212, POcp36_312, OMcp36_112, OMcp36_212, OMcp36_312, ORcp36_112, ORcp36_212, ORcp36_312, VIcp36_112, VIcp36_212, VIcp36_312;
-    double RLcp36_113, RLcp36_213, RLcp36_313, POcp36_113, POcp36_213, POcp36_313, OMcp36_113, OMcp36_213, OMcp36_313, ORcp36_113, ORcp36_213, ORcp36_313, VIcp36_113, VIcp36_213, VIcp36_313;
-
-    // variables initialization
-    q   = mbs_data->q;
-    qd  = mbs_data->qd;
-    dpt = mbs_data->dpt;
-
-    // cosines and sines
-    C4 = cos(q[4]);
-    C5 = cos(q[5]);
-    C6 = cos(q[6]);
-    C7 = cos(q[7]);
-    C8 = cos(q[8]);
-    C9 = cos(q[9]);
-    C10 = cos(q[10]);
-    C11 = cos(q[11]);
-    C12 = cos(q[12]);
-    C13 = cos(q[13]);
-    C14 = cos(q[14]);
-    C15 = cos(q[15]);
-    C16 = cos(q[16]);
-    C17 = cos(q[17]);
-    C18 = cos(q[18]);
-    C19 = cos(q[19]);
-    C20 = cos(q[20]);
-    C21 = cos(q[21]);
-    C22 = cos(q[22]);
-    C23 = cos(q[23]);
-    C24 = cos(q[24]);
-    C25 = cos(q[25]);
-    C26 = cos(q[26]);
-    C27 = cos(q[27]);
-    C28 = cos(q[28]);
-    C29 = cos(q[29]);
-    S4 = sin(q[4]);
-    S5 = sin(q[5]);
-    S6 = sin(q[6]);
-    S7 = sin(q[7]);
-    S8 = sin(q[8]);
-    S9 = sin(q[9]);
-    S10 = sin(q[10]);
-    S11 = sin(q[11]);
-    S12 = sin(q[12]);
-    S13 = sin(q[13]);
-    S14 = sin(q[14]);
-    S15 = sin(q[15]);
-    S16 = sin(q[16]);
-    S17 = sin(q[17]);
-    S18 = sin(q[18]);
-    S19 = sin(q[19]);
-    S20 = sin(q[20]);
-    S21 = sin(q[21]);
-    S22 = sin(q[22]);
-    S23 = sin(q[23]);
-    S24 = sin(q[24]);
-    S25 = sin(q[25]);
-    S26 = sin(q[26]);
-    S27 = sin(q[27]);
-    S28 = sin(q[28]);
-    S29 = sin(q[29]);
-
-    // main computation
-    ROcp25_25 = S4*S5;
-    ROcp25_35 = -C4*S5;
-    ROcp25_85 = -S4*C5;
-    ROcp25_95 = C4*C5;
-    ROcp25_16 = C5*C6;
-    ROcp25_26 = ROcp25_25*C6+C4*S6;
-    ROcp25_36 = ROcp25_35*C6+S4*S6;
-    ROcp25_46 = -C5*S6;
-    ROcp25_56 = -ROcp25_25*S6+C4*C6;
-    ROcp25_66 = -ROcp25_35*S6+S4*C6;
-    OMcp25_25 = qd[5]*C4;
-    OMcp25_35 = qd[5]*S4;
-    OMcp25_16 = qd[4]+qd[6]*S5;
-    OMcp25_26 = OMcp25_25+qd[6]*ROcp25_85;
-    OMcp25_36 = OMcp25_35+qd[6]*ROcp25_95;
-    PxF1[1] = q[1];
-    PxF1[2] = q[2];
-    PxF1[3] = q[3];
-    RxF1[1][1] = ROcp25_16;
-    RxF1[1][2] = ROcp25_26;
-    RxF1[1][3] = ROcp25_36;
-    RxF1[2][1] = ROcp25_46;
-    RxF1[2][2] = ROcp25_56;
-    RxF1[2][3] = ROcp25_66;
-    RxF1[3][1] = S5;
-    RxF1[3][2] = ROcp25_85;
-    RxF1[3][3] = ROcp25_95;
-    VxF1[1] = qd[1];
-    VxF1[2] = qd[2];
-    VxF1[3] = qd[3];
-    OMxF1[1] = OMcp25_16;
-    OMxF1[2] = OMcp25_26;
-    OMxF1[3] = OMcp25_36;
-    ROcp26_25 = S4*S5;
-    ROcp26_35 = -C4*S5;
-    ROcp26_85 = -S4*C5;
-    ROcp26_95 = C4*C5;
-    ROcp26_16 = C5*C6;
-    ROcp26_26 = ROcp26_25*C6+C4*S6;
-    ROcp26_36 = ROcp26_35*C6+S4*S6;
-    ROcp26_46 = -C5*S6;
-    ROcp26_56 = -ROcp26_25*S6+C4*C6;
-    ROcp26_66 = -ROcp26_35*S6+S4*C6;
-    ROcp26_17 = ROcp26_16*C7-S5*S7;
-    ROcp26_27 = ROcp26_26*C7-ROcp26_85*S7;
-    ROcp26_37 = ROcp26_36*C7-ROcp26_95*S7;
-    ROcp26_77 = ROcp26_16*S7+S5*C7;
-    ROcp26_87 = ROcp26_26*S7+ROcp26_85*C7;
-    ROcp26_97 = ROcp26_36*S7+ROcp26_95*C7;
-    ROcp26_48 = ROcp26_46*C8+ROcp26_77*S8;
-    ROcp26_58 = ROcp26_56*C8+ROcp26_87*S8;
-    ROcp26_68 = ROcp26_66*C8+ROcp26_97*S8;
-    ROcp26_78 = -ROcp26_46*S8+ROcp26_77*C8;
-    ROcp26_88 = -ROcp26_56*S8+ROcp26_87*C8;
-    ROcp26_98 = -ROcp26_66*S8+ROcp26_97*C8;
-    ROcp26_19 = ROcp26_17*C9+ROcp26_48*S9;
-    ROcp26_29 = ROcp26_27*C9+ROcp26_58*S9;
-    ROcp26_39 = ROcp26_37*C9+ROcp26_68*S9;
-    ROcp26_49 = -ROcp26_17*S9+ROcp26_48*C9;
-    ROcp26_59 = -ROcp26_27*S9+ROcp26_58*C9;
-    ROcp26_69 = -ROcp26_37*S9+ROcp26_68*C9;
-    OMcp26_25 = qd[5]*C4;
-    OMcp26_35 = qd[5]*S4;
-    OMcp26_16 = qd[4]+qd[6]*S5;
-    OMcp26_26 = OMcp26_25+qd[6]*ROcp26_85;
-    OMcp26_36 = OMcp26_35+qd[6]*ROcp26_95;
-    RLcp26_17 = ROcp26_46*dpt[2][2];
-    RLcp26_27 = ROcp26_56*dpt[2][2];
-    RLcp26_37 = ROcp26_66*dpt[2][2];
-    POcp26_17 = q[1]+RLcp26_17;
-    POcp26_27 = q[2]+RLcp26_27;
-    POcp26_37 = q[3]+RLcp26_37;
-    OMcp26_17 = OMcp26_16+qd[7]*ROcp26_46;
-    OMcp26_27 = OMcp26_26+qd[7]*ROcp26_56;
-    OMcp26_37 = OMcp26_36+qd[7]*ROcp26_66;
-    ORcp26_17 = OMcp26_26*RLcp26_37-OMcp26_36*RLcp26_27;
-    ORcp26_27 = -OMcp26_16*RLcp26_37+OMcp26_36*RLcp26_17;
-    ORcp26_37 = OMcp26_16*RLcp26_27-OMcp26_26*RLcp26_17;
-    VIcp26_17 = qd[1]+ORcp26_17;
-    VIcp26_27 = qd[2]+ORcp26_27;
-    VIcp26_37 = qd[3]+ORcp26_37;
-    RLcp26_18 = ROcp26_46*dpt[2][6];
-    RLcp26_28 = ROcp26_56*dpt[2][6];
-    RLcp26_38 = ROcp26_66*dpt[2][6];
-    POcp26_18 = POcp26_17+RLcp26_18;
-    POcp26_28 = POcp26_27+RLcp26_28;
-    POcp26_38 = POcp26_37+RLcp26_38;
-    OMcp26_18 = OMcp26_17+qd[8]*ROcp26_17;
-    OMcp26_28 = OMcp26_27+qd[8]*ROcp26_27;
-    OMcp26_38 = OMcp26_37+qd[8]*ROcp26_37;
-    ORcp26_18 = OMcp26_27*RLcp26_38-OMcp26_37*RLcp26_28;
-    ORcp26_28 = -OMcp26_17*RLcp26_38+OMcp26_37*RLcp26_18;
-    ORcp26_38 = OMcp26_17*RLcp26_28-OMcp26_27*RLcp26_18;
-    VIcp26_18 = ORcp26_18+VIcp26_17;
-    VIcp26_28 = ORcp26_28+VIcp26_27;
-    VIcp26_38 = ORcp26_38+VIcp26_37;
-    RLcp26_19 = ROcp26_78*dpt[3][8];
-    RLcp26_29 = ROcp26_88*dpt[3][8];
-    RLcp26_39 = ROcp26_98*dpt[3][8];
-    POcp26_19 = POcp26_18+RLcp26_19;
-    POcp26_29 = POcp26_28+RLcp26_29;
-    POcp26_39 = POcp26_38+RLcp26_39;
-    OMcp26_19 = OMcp26_18+qd[9]*ROcp26_78;
-    OMcp26_29 = OMcp26_28+qd[9]*ROcp26_88;
-    OMcp26_39 = OMcp26_38+qd[9]*ROcp26_98;
-    ORcp26_19 = OMcp26_28*RLcp26_39-OMcp26_38*RLcp26_29;
-    ORcp26_29 = -OMcp26_18*RLcp26_39+OMcp26_38*RLcp26_19;
-    ORcp26_39 = OMcp26_18*RLcp26_29-OMcp26_28*RLcp26_19;
-    VIcp26_19 = ORcp26_19+VIcp26_18;
-    VIcp26_29 = ORcp26_29+VIcp26_28;
-    VIcp26_39 = ORcp26_39+VIcp26_38;
-    PxF2[1] = POcp26_19;
-    PxF2[2] = POcp26_29;
-    PxF2[3] = POcp26_39;
-    RxF2[1][1] = ROcp26_19;
-    RxF2[1][2] = ROcp26_29;
-    RxF2[1][3] = ROcp26_39;
-    RxF2[2][1] = ROcp26_49;
-    RxF2[2][2] = ROcp26_59;
-    RxF2[2][3] = ROcp26_69;
-    RxF2[3][1] = ROcp26_78;
-    RxF2[3][2] = ROcp26_88;
-    RxF2[3][3] = ROcp26_98;
-    VxF2[1] = VIcp26_19;
-    VxF2[2] = VIcp26_29;
-    VxF2[3] = VIcp26_39;
-    OMxF2[1] = OMcp26_19;
-    OMxF2[2] = OMcp26_29;
-    OMxF2[3] = OMcp26_39;
-    ROcp27_25 = S4*S5;
-    ROcp27_35 = -C4*S5;
-    ROcp27_85 = -S4*C5;
-    ROcp27_95 = C4*C5;
-    ROcp27_16 = C5*C6;
-    ROcp27_26 = ROcp27_25*C6+C4*S6;
-    ROcp27_36 = ROcp27_35*C6+S4*S6;
-    ROcp27_46 = -C5*S6;
-    ROcp27_56 = -ROcp27_25*S6+C4*C6;
-    ROcp27_66 = -ROcp27_35*S6+S4*C6;
-    ROcp27_17 = ROcp27_16*C7-S5*S7;
-    ROcp27_27 = ROcp27_26*C7-ROcp27_85*S7;
-    ROcp27_37 = ROcp27_36*C7-ROcp27_95*S7;
-    ROcp27_77 = ROcp27_16*S7+S5*C7;
-    ROcp27_87 = ROcp27_26*S7+ROcp27_85*C7;
-    ROcp27_97 = ROcp27_36*S7+ROcp27_95*C7;
-    ROcp27_48 = ROcp27_46*C8+ROcp27_77*S8;
-    ROcp27_58 = ROcp27_56*C8+ROcp27_87*S8;
-    ROcp27_68 = ROcp27_66*C8+ROcp27_97*S8;
-    ROcp27_78 = -ROcp27_46*S8+ROcp27_77*C8;
-    ROcp27_88 = -ROcp27_56*S8+ROcp27_87*C8;
-    ROcp27_98 = -ROcp27_66*S8+ROcp27_97*C8;
-    ROcp27_19 = ROcp27_17*C9+ROcp27_48*S9;
-    ROcp27_29 = ROcp27_27*C9+ROcp27_58*S9;
-    ROcp27_39 = ROcp27_37*C9+ROcp27_68*S9;
-    ROcp27_49 = -ROcp27_17*S9+ROcp27_48*C9;
-    ROcp27_59 = -ROcp27_27*S9+ROcp27_58*C9;
-    ROcp27_69 = -ROcp27_37*S9+ROcp27_68*C9;
-    ROcp27_110 = ROcp27_19*C10-ROcp27_78*S10;
-    ROcp27_210 = ROcp27_29*C10-ROcp27_88*S10;
-    ROcp27_310 = ROcp27_39*C10-ROcp27_98*S10;
-    ROcp27_710 = ROcp27_19*S10+ROcp27_78*C10;
-    ROcp27_810 = ROcp27_29*S10+ROcp27_88*C10;
-    ROcp27_910 = ROcp27_39*S10+ROcp27_98*C10;
-    OMcp27_25 = qd[5]*C4;
-    OMcp27_35 = qd[5]*S4;
-    OMcp27_16 = qd[4]+qd[6]*S5;
-    OMcp27_26 = OMcp27_25+qd[6]*ROcp27_85;
-    OMcp27_36 = OMcp27_35+qd[6]*ROcp27_95;
-    RLcp27_17 = ROcp27_46*dpt[2][2];
-    RLcp27_27 = ROcp27_56*dpt[2][2];
-    RLcp27_37 = ROcp27_66*dpt[2][2];
-    POcp27_17 = q[1]+RLcp27_17;
-    POcp27_27 = q[2]+RLcp27_27;
-    POcp27_37 = q[3]+RLcp27_37;
-    OMcp27_17 = OMcp27_16+qd[7]*ROcp27_46;
-    OMcp27_27 = OMcp27_26+qd[7]*ROcp27_56;
-    OMcp27_37 = OMcp27_36+qd[7]*ROcp27_66;
-    ORcp27_17 = OMcp27_26*RLcp27_37-OMcp27_36*RLcp27_27;
-    ORcp27_27 = -OMcp27_16*RLcp27_37+OMcp27_36*RLcp27_17;
-    ORcp27_37 = OMcp27_16*RLcp27_27-OMcp27_26*RLcp27_17;
-    VIcp27_17 = qd[1]+ORcp27_17;
-    VIcp27_27 = qd[2]+ORcp27_27;
-    VIcp27_37 = qd[3]+ORcp27_37;
-    RLcp27_18 = ROcp27_46*dpt[2][6];
-    RLcp27_28 = ROcp27_56*dpt[2][6];
-    RLcp27_38 = ROcp27_66*dpt[2][6];
-    POcp27_18 = POcp27_17+RLcp27_18;
-    POcp27_28 = POcp27_27+RLcp27_28;
-    POcp27_38 = POcp27_37+RLcp27_38;
-    OMcp27_18 = OMcp27_17+qd[8]*ROcp27_17;
-    OMcp27_28 = OMcp27_27+qd[8]*ROcp27_27;
-    OMcp27_38 = OMcp27_37+qd[8]*ROcp27_37;
-    ORcp27_18 = OMcp27_27*RLcp27_38-OMcp27_37*RLcp27_28;
-    ORcp27_28 = -OMcp27_17*RLcp27_38+OMcp27_37*RLcp27_18;
-    ORcp27_38 = OMcp27_17*RLcp27_28-OMcp27_27*RLcp27_18;
-    VIcp27_18 = ORcp27_18+VIcp27_17;
-    VIcp27_28 = ORcp27_28+VIcp27_27;
-    VIcp27_38 = ORcp27_38+VIcp27_37;
-    RLcp27_19 = ROcp27_78*dpt[3][8];
-    RLcp27_29 = ROcp27_88*dpt[3][8];
-    RLcp27_39 = ROcp27_98*dpt[3][8];
-    POcp27_19 = POcp27_18+RLcp27_19;
-    POcp27_29 = POcp27_28+RLcp27_29;
-    POcp27_39 = POcp27_38+RLcp27_39;
-    OMcp27_19 = OMcp27_18+qd[9]*ROcp27_78;
-    OMcp27_29 = OMcp27_28+qd[9]*ROcp27_88;
-    OMcp27_39 = OMcp27_38+qd[9]*ROcp27_98;
-    ORcp27_19 = OMcp27_28*RLcp27_39-OMcp27_38*RLcp27_29;
-    ORcp27_29 = -OMcp27_18*RLcp27_39+OMcp27_38*RLcp27_19;
-    ORcp27_39 = OMcp27_18*RLcp27_29-OMcp27_28*RLcp27_19;
-    VIcp27_19 = ORcp27_19+VIcp27_18;
-    VIcp27_29 = ORcp27_29+VIcp27_28;
-    VIcp27_39 = ORcp27_39+VIcp27_38;
-    RLcp27_110 = ROcp27_78*dpt[3][11];
-    RLcp27_210 = ROcp27_88*dpt[3][11];
-    RLcp27_310 = ROcp27_98*dpt[3][11];
-    POcp27_110 = POcp27_19+RLcp27_110;
-    POcp27_210 = POcp27_29+RLcp27_210;
-    POcp27_310 = POcp27_39+RLcp27_310;
-    OMcp27_110 = OMcp27_19+qd[10]*ROcp27_49;
-    OMcp27_210 = OMcp27_29+qd[10]*ROcp27_59;
-    OMcp27_310 = OMcp27_39+qd[10]*ROcp27_69;
-    ORcp27_110 = OMcp27_29*RLcp27_310-OMcp27_39*RLcp27_210;
-    ORcp27_210 = -OMcp27_19*RLcp27_310+OMcp27_39*RLcp27_110;
-    ORcp27_310 = OMcp27_19*RLcp27_210-OMcp27_29*RLcp27_110;
-    VIcp27_110 = ORcp27_110+VIcp27_19;
-    VIcp27_210 = ORcp27_210+VIcp27_29;
-    VIcp27_310 = ORcp27_310+VIcp27_39;
-    PxF3[1] = POcp27_110;
-    PxF3[2] = POcp27_210;
-    PxF3[3] = POcp27_310;
-    RxF3[1][1] = ROcp27_110;
-    RxF3[1][2] = ROcp27_210;
-    RxF3[1][3] = ROcp27_310;
-    RxF3[2][1] = ROcp27_49;
-    RxF3[2][2] = ROcp27_59;
-    RxF3[2][3] = ROcp27_69;
-    RxF3[3][1] = ROcp27_710;
-    RxF3[3][2] = ROcp27_810;
-    RxF3[3][3] = ROcp27_910;
-    VxF3[1] = VIcp27_110;
-    VxF3[2] = VIcp27_210;
-    VxF3[3] = VIcp27_310;
-    OMxF3[1] = OMcp27_110;
-    OMxF3[2] = OMcp27_210;
-    OMxF3[3] = OMcp27_310;
-    ROcp28_25 = S4*S5;
-    ROcp28_35 = -C4*S5;
-    ROcp28_85 = -S4*C5;
-    ROcp28_95 = C4*C5;
-    ROcp28_16 = C5*C6;
-    ROcp28_26 = ROcp28_25*C6+C4*S6;
-    ROcp28_36 = ROcp28_35*C6+S4*S6;
-    ROcp28_46 = -C5*S6;
-    ROcp28_56 = -ROcp28_25*S6+C4*C6;
-    ROcp28_66 = -ROcp28_35*S6+S4*C6;
-    ROcp28_17 = ROcp28_16*C7-S5*S7;
-    ROcp28_27 = ROcp28_26*C7-ROcp28_85*S7;
-    ROcp28_37 = ROcp28_36*C7-ROcp28_95*S7;
-    ROcp28_77 = ROcp28_16*S7+S5*C7;
-    ROcp28_87 = ROcp28_26*S7+ROcp28_85*C7;
-    ROcp28_97 = ROcp28_36*S7+ROcp28_95*C7;
-    ROcp28_48 = ROcp28_46*C8+ROcp28_77*S8;
-    ROcp28_58 = ROcp28_56*C8+ROcp28_87*S8;
-    ROcp28_68 = ROcp28_66*C8+ROcp28_97*S8;
-    ROcp28_78 = -ROcp28_46*S8+ROcp28_77*C8;
-    ROcp28_88 = -ROcp28_56*S8+ROcp28_87*C8;
-    ROcp28_98 = -ROcp28_66*S8+ROcp28_97*C8;
-    ROcp28_19 = ROcp28_17*C9+ROcp28_48*S9;
-    ROcp28_29 = ROcp28_27*C9+ROcp28_58*S9;
-    ROcp28_39 = ROcp28_37*C9+ROcp28_68*S9;
-    ROcp28_49 = -ROcp28_17*S9+ROcp28_48*C9;
-    ROcp28_59 = -ROcp28_27*S9+ROcp28_58*C9;
-    ROcp28_69 = -ROcp28_37*S9+ROcp28_68*C9;
-    ROcp28_110 = ROcp28_19*C10-ROcp28_78*S10;
-    ROcp28_210 = ROcp28_29*C10-ROcp28_88*S10;
-    ROcp28_310 = ROcp28_39*C10-ROcp28_98*S10;
-    ROcp28_710 = ROcp28_19*S10+ROcp28_78*C10;
-    ROcp28_810 = ROcp28_29*S10+ROcp28_88*C10;
-    ROcp28_910 = ROcp28_39*S10+ROcp28_98*C10;
-    ROcp28_411 = ROcp28_49*C11+ROcp28_710*S11;
-    ROcp28_511 = ROcp28_59*C11+ROcp28_810*S11;
-    ROcp28_611 = ROcp28_69*C11+ROcp28_910*S11;
-    ROcp28_711 = -ROcp28_49*S11+ROcp28_710*C11;
-    ROcp28_811 = -ROcp28_59*S11+ROcp28_810*C11;
-    ROcp28_911 = -ROcp28_69*S11+ROcp28_910*C11;
-    ROcp28_112 = ROcp28_110*C12-ROcp28_711*S12;
-    ROcp28_212 = ROcp28_210*C12-ROcp28_811*S12;
-    ROcp28_312 = ROcp28_310*C12-ROcp28_911*S12;
-    ROcp28_712 = ROcp28_110*S12+ROcp28_711*C12;
-    ROcp28_812 = ROcp28_210*S12+ROcp28_811*C12;
-    ROcp28_912 = ROcp28_310*S12+ROcp28_911*C12;
-    OMcp28_25 = qd[5]*C4;
-    OMcp28_35 = qd[5]*S4;
-    OMcp28_16 = qd[4]+qd[6]*S5;
-    OMcp28_26 = OMcp28_25+qd[6]*ROcp28_85;
-    OMcp28_36 = OMcp28_35+qd[6]*ROcp28_95;
-    RLcp28_17 = ROcp28_46*dpt[2][2];
-    RLcp28_27 = ROcp28_56*dpt[2][2];
-    RLcp28_37 = ROcp28_66*dpt[2][2];
-    POcp28_17 = q[1]+RLcp28_17;
-    POcp28_27 = q[2]+RLcp28_27;
-    POcp28_37 = q[3]+RLcp28_37;
-    OMcp28_17 = OMcp28_16+qd[7]*ROcp28_46;
-    OMcp28_27 = OMcp28_26+qd[7]*ROcp28_56;
-    OMcp28_37 = OMcp28_36+qd[7]*ROcp28_66;
-    ORcp28_17 = OMcp28_26*RLcp28_37-OMcp28_36*RLcp28_27;
-    ORcp28_27 = -OMcp28_16*RLcp28_37+OMcp28_36*RLcp28_17;
-    ORcp28_37 = OMcp28_16*RLcp28_27-OMcp28_26*RLcp28_17;
-    VIcp28_17 = qd[1]+ORcp28_17;
-    VIcp28_27 = qd[2]+ORcp28_27;
-    VIcp28_37 = qd[3]+ORcp28_37;
-    RLcp28_18 = ROcp28_46*dpt[2][6];
-    RLcp28_28 = ROcp28_56*dpt[2][6];
-    RLcp28_38 = ROcp28_66*dpt[2][6];
-    POcp28_18 = POcp28_17+RLcp28_18;
-    POcp28_28 = POcp28_27+RLcp28_28;
-    POcp28_38 = POcp28_37+RLcp28_38;
-    OMcp28_18 = OMcp28_17+qd[8]*ROcp28_17;
-    OMcp28_28 = OMcp28_27+qd[8]*ROcp28_27;
-    OMcp28_38 = OMcp28_37+qd[8]*ROcp28_37;
-    ORcp28_18 = OMcp28_27*RLcp28_38-OMcp28_37*RLcp28_28;
-    ORcp28_28 = -OMcp28_17*RLcp28_38+OMcp28_37*RLcp28_18;
-    ORcp28_38 = OMcp28_17*RLcp28_28-OMcp28_27*RLcp28_18;
-    VIcp28_18 = ORcp28_18+VIcp28_17;
-    VIcp28_28 = ORcp28_28+VIcp28_27;
-    VIcp28_38 = ORcp28_38+VIcp28_37;
-    RLcp28_19 = ROcp28_78*dpt[3][8];
-    RLcp28_29 = ROcp28_88*dpt[3][8];
-    RLcp28_39 = ROcp28_98*dpt[3][8];
-    POcp28_19 = POcp28_18+RLcp28_19;
-    POcp28_29 = POcp28_28+RLcp28_29;
-    POcp28_39 = POcp28_38+RLcp28_39;
-    OMcp28_19 = OMcp28_18+qd[9]*ROcp28_78;
-    OMcp28_29 = OMcp28_28+qd[9]*ROcp28_88;
-    OMcp28_39 = OMcp28_38+qd[9]*ROcp28_98;
-    ORcp28_19 = OMcp28_28*RLcp28_39-OMcp28_38*RLcp28_29;
-    ORcp28_29 = -OMcp28_18*RLcp28_39+OMcp28_38*RLcp28_19;
-    ORcp28_39 = OMcp28_18*RLcp28_29-OMcp28_28*RLcp28_19;
-    VIcp28_19 = ORcp28_19+VIcp28_18;
-    VIcp28_29 = ORcp28_29+VIcp28_28;
-    VIcp28_39 = ORcp28_39+VIcp28_38;
-    RLcp28_110 = ROcp28_78*dpt[3][11];
-    RLcp28_210 = ROcp28_88*dpt[3][11];
-    RLcp28_310 = ROcp28_98*dpt[3][11];
-    POcp28_110 = POcp28_19+RLcp28_110;
-    POcp28_210 = POcp28_29+RLcp28_210;
-    POcp28_310 = POcp28_39+RLcp28_310;
-    OMcp28_110 = OMcp28_19+qd[10]*ROcp28_49;
-    OMcp28_210 = OMcp28_29+qd[10]*ROcp28_59;
-    OMcp28_310 = OMcp28_39+qd[10]*ROcp28_69;
-    ORcp28_110 = OMcp28_29*RLcp28_310-OMcp28_39*RLcp28_210;
-    ORcp28_210 = -OMcp28_19*RLcp28_310+OMcp28_39*RLcp28_110;
-    ORcp28_310 = OMcp28_19*RLcp28_210-OMcp28_29*RLcp28_110;
-    VIcp28_110 = ORcp28_110+VIcp28_19;
-    VIcp28_210 = ORcp28_210+VIcp28_29;
-    VIcp28_310 = ORcp28_310+VIcp28_39;
-    RLcp28_111 = ROcp28_710*dpt[3][14];
-    RLcp28_211 = ROcp28_810*dpt[3][14];
-    RLcp28_311 = ROcp28_910*dpt[3][14];
-    POcp28_111 = POcp28_110+RLcp28_111;
-    POcp28_211 = POcp28_210+RLcp28_211;
-    POcp28_311 = POcp28_310+RLcp28_311;
-    OMcp28_111 = OMcp28_110+qd[11]*ROcp28_110;
-    OMcp28_211 = OMcp28_210+qd[11]*ROcp28_210;
-    OMcp28_311 = OMcp28_310+qd[11]*ROcp28_310;
-    ORcp28_111 = OMcp28_210*RLcp28_311-OMcp28_310*RLcp28_211;
-    ORcp28_211 = -OMcp28_110*RLcp28_311+OMcp28_310*RLcp28_111;
-    ORcp28_311 = OMcp28_110*RLcp28_211-OMcp28_210*RLcp28_111;
-    VIcp28_111 = ORcp28_111+VIcp28_110;
-    VIcp28_211 = ORcp28_211+VIcp28_210;
-    VIcp28_311 = ORcp28_311+VIcp28_310;
-    OMcp28_112 = OMcp28_111+qd[12]*ROcp28_411;
-    OMcp28_212 = OMcp28_211+qd[12]*ROcp28_511;
-    OMcp28_312 = OMcp28_311+qd[12]*ROcp28_611;
-    RLcp28_113 = ROcp28_712*dpt[3][19];
-    RLcp28_213 = ROcp28_812*dpt[3][19];
-    RLcp28_313 = ROcp28_912*dpt[3][19];
-    POcp28_113 = POcp28_111+RLcp28_113;
-    POcp28_213 = POcp28_211+RLcp28_213;
-    POcp28_313 = POcp28_311+RLcp28_313;
-    ORcp28_113 = OMcp28_212*RLcp28_313-OMcp28_312*RLcp28_213;
-    ORcp28_213 = -OMcp28_112*RLcp28_313+OMcp28_312*RLcp28_113;
-    ORcp28_313 = OMcp28_112*RLcp28_213-OMcp28_212*RLcp28_113;
-    VIcp28_113 = ORcp28_113+VIcp28_111;
-    VIcp28_213 = ORcp28_213+VIcp28_211;
-    VIcp28_313 = ORcp28_313+VIcp28_311;
-    PxF4[1] = POcp28_113;
-    PxF4[2] = POcp28_213;
-    PxF4[3] = POcp28_313;
-    RxF4[1][1] = ROcp28_112;
-    RxF4[1][2] = ROcp28_212;
-    RxF4[1][3] = ROcp28_312;
-    RxF4[2][1] = ROcp28_411;
-    RxF4[2][2] = ROcp28_511;
-    RxF4[2][3] = ROcp28_611;
-    RxF4[3][1] = ROcp28_712;
-    RxF4[3][2] = ROcp28_812;
-    RxF4[3][3] = ROcp28_912;
-    VxF4[1] = VIcp28_113;
-    VxF4[2] = VIcp28_213;
-    VxF4[3] = VIcp28_313;
-    OMxF4[1] = OMcp28_112;
-    OMxF4[2] = OMcp28_212;
-    OMxF4[3] = OMcp28_312;
-    ROcp29_25 = S4*S5;
-    ROcp29_35 = -C4*S5;
-    ROcp29_85 = -S4*C5;
-    ROcp29_95 = C4*C5;
-    ROcp29_16 = C5*C6;
-    ROcp29_26 = ROcp29_25*C6+C4*S6;
-    ROcp29_36 = ROcp29_35*C6+S4*S6;
-    ROcp29_46 = -C5*S6;
-    ROcp29_56 = -ROcp29_25*S6+C4*C6;
-    ROcp29_66 = -ROcp29_35*S6+S4*C6;
-    ROcp29_113 = ROcp29_16*C13-S13*S5;
-    ROcp29_213 = ROcp29_26*C13-ROcp29_85*S13;
-    ROcp29_313 = ROcp29_36*C13-ROcp29_95*S13;
-    ROcp29_713 = ROcp29_16*S13+C13*S5;
-    ROcp29_813 = ROcp29_26*S13+ROcp29_85*C13;
-    ROcp29_913 = ROcp29_36*S13+ROcp29_95*C13;
-    ROcp29_414 = ROcp29_46*C14+ROcp29_713*S14;
-    ROcp29_514 = ROcp29_56*C14+ROcp29_813*S14;
-    ROcp29_614 = ROcp29_66*C14+ROcp29_913*S14;
-    ROcp29_714 = -ROcp29_46*S14+ROcp29_713*C14;
-    ROcp29_814 = -ROcp29_56*S14+ROcp29_813*C14;
-    ROcp29_914 = -ROcp29_66*S14+ROcp29_913*C14;
-    ROcp29_115 = ROcp29_113*C15+ROcp29_414*S15;
-    ROcp29_215 = ROcp29_213*C15+ROcp29_514*S15;
-    ROcp29_315 = ROcp29_313*C15+ROcp29_614*S15;
-    ROcp29_415 = -ROcp29_113*S15+ROcp29_414*C15;
-    ROcp29_515 = -ROcp29_213*S15+ROcp29_514*C15;
-    ROcp29_615 = -ROcp29_313*S15+ROcp29_614*C15;
-    OMcp29_25 = qd[5]*C4;
-    OMcp29_35 = qd[5]*S4;
-    OMcp29_16 = qd[4]+qd[6]*S5;
-    OMcp29_26 = OMcp29_25+qd[6]*ROcp29_85;
-    OMcp29_36 = OMcp29_35+qd[6]*ROcp29_95;
-    RLcp29_17 = ROcp29_46*dpt[2][3];
-    RLcp29_27 = ROcp29_56*dpt[2][3];
-    RLcp29_37 = ROcp29_66*dpt[2][3];
-    POcp29_17 = q[1]+RLcp29_17;
-    POcp29_27 = q[2]+RLcp29_27;
-    POcp29_37 = q[3]+RLcp29_37;
-    OMcp29_17 = OMcp29_16+qd[13]*ROcp29_46;
-    OMcp29_27 = OMcp29_26+qd[13]*ROcp29_56;
-    OMcp29_37 = OMcp29_36+qd[13]*ROcp29_66;
-    ORcp29_17 = OMcp29_26*RLcp29_37-OMcp29_36*RLcp29_27;
-    ORcp29_27 = -OMcp29_16*RLcp29_37+OMcp29_36*RLcp29_17;
-    ORcp29_37 = OMcp29_16*RLcp29_27-OMcp29_26*RLcp29_17;
-    VIcp29_17 = qd[1]+ORcp29_17;
-    VIcp29_27 = qd[2]+ORcp29_27;
-    VIcp29_37 = qd[3]+ORcp29_37;
-    RLcp29_18 = ROcp29_46*dpt[2][20];
-    RLcp29_28 = ROcp29_56*dpt[2][20];
-    RLcp29_38 = ROcp29_66*dpt[2][20];
-    POcp29_18 = POcp29_17+RLcp29_18;
-    POcp29_28 = POcp29_27+RLcp29_28;
-    POcp29_38 = POcp29_37+RLcp29_38;
-    OMcp29_18 = OMcp29_17+qd[14]*ROcp29_113;
-    OMcp29_28 = OMcp29_27+qd[14]*ROcp29_213;
-    OMcp29_38 = OMcp29_37+qd[14]*ROcp29_313;
-    ORcp29_18 = OMcp29_27*RLcp29_38-OMcp29_37*RLcp29_28;
-    ORcp29_28 = -OMcp29_17*RLcp29_38+OMcp29_37*RLcp29_18;
-    ORcp29_38 = OMcp29_17*RLcp29_28-OMcp29_27*RLcp29_18;
-    VIcp29_18 = ORcp29_18+VIcp29_17;
-    VIcp29_28 = ORcp29_28+VIcp29_27;
-    VIcp29_38 = ORcp29_38+VIcp29_37;
-    RLcp29_19 = ROcp29_714*dpt[3][22];
-    RLcp29_29 = ROcp29_814*dpt[3][22];
-    RLcp29_39 = ROcp29_914*dpt[3][22];
-    POcp29_19 = POcp29_18+RLcp29_19;
-    POcp29_29 = POcp29_28+RLcp29_29;
-    POcp29_39 = POcp29_38+RLcp29_39;
-    OMcp29_19 = OMcp29_18+qd[15]*ROcp29_714;
-    OMcp29_29 = OMcp29_28+qd[15]*ROcp29_814;
-    OMcp29_39 = OMcp29_38+qd[15]*ROcp29_914;
-    ORcp29_19 = OMcp29_28*RLcp29_39-OMcp29_38*RLcp29_29;
-    ORcp29_29 = -OMcp29_18*RLcp29_39+OMcp29_38*RLcp29_19;
-    ORcp29_39 = OMcp29_18*RLcp29_29-OMcp29_28*RLcp29_19;
-    VIcp29_19 = ORcp29_19+VIcp29_18;
-    VIcp29_29 = ORcp29_29+VIcp29_28;
-    VIcp29_39 = ORcp29_39+VIcp29_38;
-    PxF5[1] = POcp29_19;
-    PxF5[2] = POcp29_29;
-    PxF5[3] = POcp29_39;
-    RxF5[1][1] = ROcp29_115;
-    RxF5[1][2] = ROcp29_215;
-    RxF5[1][3] = ROcp29_315;
-    RxF5[2][1] = ROcp29_415;
-    RxF5[2][2] = ROcp29_515;
-    RxF5[2][3] = ROcp29_615;
-    RxF5[3][1] = ROcp29_714;
-    RxF5[3][2] = ROcp29_814;
-    RxF5[3][3] = ROcp29_914;
-    VxF5[1] = VIcp29_19;
-    VxF5[2] = VIcp29_29;
-    VxF5[3] = VIcp29_39;
-    OMxF5[1] = OMcp29_19;
-    OMxF5[2] = OMcp29_29;
-    OMxF5[3] = OMcp29_39;
-    ROcp30_25 = S4*S5;
-    ROcp30_35 = -C4*S5;
-    ROcp30_85 = -S4*C5;
-    ROcp30_95 = C4*C5;
-    ROcp30_16 = C5*C6;
-    ROcp30_26 = ROcp30_25*C6+C4*S6;
-    ROcp30_36 = ROcp30_35*C6+S4*S6;
-    ROcp30_46 = -C5*S6;
-    ROcp30_56 = -ROcp30_25*S6+C4*C6;
-    ROcp30_66 = -ROcp30_35*S6+S4*C6;
-    ROcp30_113 = ROcp30_16*C13-S13*S5;
-    ROcp30_213 = ROcp30_26*C13-ROcp30_85*S13;
-    ROcp30_313 = ROcp30_36*C13-ROcp30_95*S13;
-    ROcp30_713 = ROcp30_16*S13+C13*S5;
-    ROcp30_813 = ROcp30_26*S13+ROcp30_85*C13;
-    ROcp30_913 = ROcp30_36*S13+ROcp30_95*C13;
-    ROcp30_414 = ROcp30_46*C14+ROcp30_713*S14;
-    ROcp30_514 = ROcp30_56*C14+ROcp30_813*S14;
-    ROcp30_614 = ROcp30_66*C14+ROcp30_913*S14;
-    ROcp30_714 = -ROcp30_46*S14+ROcp30_713*C14;
-    ROcp30_814 = -ROcp30_56*S14+ROcp30_813*C14;
-    ROcp30_914 = -ROcp30_66*S14+ROcp30_913*C14;
-    ROcp30_115 = ROcp30_113*C15+ROcp30_414*S15;
-    ROcp30_215 = ROcp30_213*C15+ROcp30_514*S15;
-    ROcp30_315 = ROcp30_313*C15+ROcp30_614*S15;
-    ROcp30_415 = -ROcp30_113*S15+ROcp30_414*C15;
-    ROcp30_515 = -ROcp30_213*S15+ROcp30_514*C15;
-    ROcp30_615 = -ROcp30_313*S15+ROcp30_614*C15;
-    ROcp30_116 = ROcp30_115*C16-ROcp30_714*S16;
-    ROcp30_216 = ROcp30_215*C16-ROcp30_814*S16;
-    ROcp30_316 = ROcp30_315*C16-ROcp30_914*S16;
-    ROcp30_716 = ROcp30_115*S16+ROcp30_714*C16;
-    ROcp30_816 = ROcp30_215*S16+ROcp30_814*C16;
-    ROcp30_916 = ROcp30_315*S16+ROcp30_914*C16;
-    OMcp30_25 = qd[5]*C4;
-    OMcp30_35 = qd[5]*S4;
-    OMcp30_16 = qd[4]+qd[6]*S5;
-    OMcp30_26 = OMcp30_25+qd[6]*ROcp30_85;
-    OMcp30_36 = OMcp30_35+qd[6]*ROcp30_95;
-    RLcp30_17 = ROcp30_46*dpt[2][3];
-    RLcp30_27 = ROcp30_56*dpt[2][3];
-    RLcp30_37 = ROcp30_66*dpt[2][3];
-    POcp30_17 = q[1]+RLcp30_17;
-    POcp30_27 = q[2]+RLcp30_27;
-    POcp30_37 = q[3]+RLcp30_37;
-    OMcp30_17 = OMcp30_16+qd[13]*ROcp30_46;
-    OMcp30_27 = OMcp30_26+qd[13]*ROcp30_56;
-    OMcp30_37 = OMcp30_36+qd[13]*ROcp30_66;
-    ORcp30_17 = OMcp30_26*RLcp30_37-OMcp30_36*RLcp30_27;
-    ORcp30_27 = -OMcp30_16*RLcp30_37+OMcp30_36*RLcp30_17;
-    ORcp30_37 = OMcp30_16*RLcp30_27-OMcp30_26*RLcp30_17;
-    VIcp30_17 = qd[1]+ORcp30_17;
-    VIcp30_27 = qd[2]+ORcp30_27;
-    VIcp30_37 = qd[3]+ORcp30_37;
-    RLcp30_18 = ROcp30_46*dpt[2][20];
-    RLcp30_28 = ROcp30_56*dpt[2][20];
-    RLcp30_38 = ROcp30_66*dpt[2][20];
-    POcp30_18 = POcp30_17+RLcp30_18;
-    POcp30_28 = POcp30_27+RLcp30_28;
-    POcp30_38 = POcp30_37+RLcp30_38;
-    OMcp30_18 = OMcp30_17+qd[14]*ROcp30_113;
-    OMcp30_28 = OMcp30_27+qd[14]*ROcp30_213;
-    OMcp30_38 = OMcp30_37+qd[14]*ROcp30_313;
-    ORcp30_18 = OMcp30_27*RLcp30_38-OMcp30_37*RLcp30_28;
-    ORcp30_28 = -OMcp30_17*RLcp30_38+OMcp30_37*RLcp30_18;
-    ORcp30_38 = OMcp30_17*RLcp30_28-OMcp30_27*RLcp30_18;
-    VIcp30_18 = ORcp30_18+VIcp30_17;
-    VIcp30_28 = ORcp30_28+VIcp30_27;
-    VIcp30_38 = ORcp30_38+VIcp30_37;
-    RLcp30_19 = ROcp30_714*dpt[3][22];
-    RLcp30_29 = ROcp30_814*dpt[3][22];
-    RLcp30_39 = ROcp30_914*dpt[3][22];
-    POcp30_19 = POcp30_18+RLcp30_19;
-    POcp30_29 = POcp30_28+RLcp30_29;
-    POcp30_39 = POcp30_38+RLcp30_39;
-    OMcp30_19 = OMcp30_18+qd[15]*ROcp30_714;
-    OMcp30_29 = OMcp30_28+qd[15]*ROcp30_814;
-    OMcp30_39 = OMcp30_38+qd[15]*ROcp30_914;
-    ORcp30_19 = OMcp30_28*RLcp30_39-OMcp30_38*RLcp30_29;
-    ORcp30_29 = -OMcp30_18*RLcp30_39+OMcp30_38*RLcp30_19;
-    ORcp30_39 = OMcp30_18*RLcp30_29-OMcp30_28*RLcp30_19;
-    VIcp30_19 = ORcp30_19+VIcp30_18;
-    VIcp30_29 = ORcp30_29+VIcp30_28;
-    VIcp30_39 = ORcp30_39+VIcp30_38;
-    RLcp30_110 = ROcp30_714*dpt[3][25];
-    RLcp30_210 = ROcp30_814*dpt[3][25];
-    RLcp30_310 = ROcp30_914*dpt[3][25];
-    POcp30_110 = POcp30_19+RLcp30_110;
-    POcp30_210 = POcp30_29+RLcp30_210;
-    POcp30_310 = POcp30_39+RLcp30_310;
-    OMcp30_110 = OMcp30_19+qd[16]*ROcp30_415;
-    OMcp30_210 = OMcp30_29+qd[16]*ROcp30_515;
-    OMcp30_310 = OMcp30_39+qd[16]*ROcp30_615;
-    ORcp30_110 = OMcp30_29*RLcp30_310-OMcp30_39*RLcp30_210;
-    ORcp30_210 = -OMcp30_19*RLcp30_310+OMcp30_39*RLcp30_110;
-    ORcp30_310 = OMcp30_19*RLcp30_210-OMcp30_29*RLcp30_110;
-    VIcp30_110 = ORcp30_110+VIcp30_19;
-    VIcp30_210 = ORcp30_210+VIcp30_29;
-    VIcp30_310 = ORcp30_310+VIcp30_39;
-    PxF6[1] = POcp30_110;
-    PxF6[2] = POcp30_210;
-    PxF6[3] = POcp30_310;
-    RxF6[1][1] = ROcp30_116;
-    RxF6[1][2] = ROcp30_216;
-    RxF6[1][3] = ROcp30_316;
-    RxF6[2][1] = ROcp30_415;
-    RxF6[2][2] = ROcp30_515;
-    RxF6[2][3] = ROcp30_615;
-    RxF6[3][1] = ROcp30_716;
-    RxF6[3][2] = ROcp30_816;
-    RxF6[3][3] = ROcp30_916;
-    VxF6[1] = VIcp30_110;
-    VxF6[2] = VIcp30_210;
-    VxF6[3] = VIcp30_310;
-    OMxF6[1] = OMcp30_110;
-    OMxF6[2] = OMcp30_210;
-    OMxF6[3] = OMcp30_310;
-    ROcp31_25 = S4*S5;
-    ROcp31_35 = -C4*S5;
-    ROcp31_85 = -S4*C5;
-    ROcp31_95 = C4*C5;
-    ROcp31_16 = C5*C6;
-    ROcp31_26 = ROcp31_25*C6+C4*S6;
-    ROcp31_36 = ROcp31_35*C6+S4*S6;
-    ROcp31_46 = -C5*S6;
-    ROcp31_56 = -ROcp31_25*S6+C4*C6;
-    ROcp31_66 = -ROcp31_35*S6+S4*C6;
-    ROcp31_113 = ROcp31_16*C13-S13*S5;
-    ROcp31_213 = ROcp31_26*C13-ROcp31_85*S13;
-    ROcp31_313 = ROcp31_36*C13-ROcp31_95*S13;
-    ROcp31_713 = ROcp31_16*S13+C13*S5;
-    ROcp31_813 = ROcp31_26*S13+ROcp31_85*C13;
-    ROcp31_913 = ROcp31_36*S13+ROcp31_95*C13;
-    ROcp31_414 = ROcp31_46*C14+ROcp31_713*S14;
-    ROcp31_514 = ROcp31_56*C14+ROcp31_813*S14;
-    ROcp31_614 = ROcp31_66*C14+ROcp31_913*S14;
-    ROcp31_714 = -ROcp31_46*S14+ROcp31_713*C14;
-    ROcp31_814 = -ROcp31_56*S14+ROcp31_813*C14;
-    ROcp31_914 = -ROcp31_66*S14+ROcp31_913*C14;
-    ROcp31_115 = ROcp31_113*C15+ROcp31_414*S15;
-    ROcp31_215 = ROcp31_213*C15+ROcp31_514*S15;
-    ROcp31_315 = ROcp31_313*C15+ROcp31_614*S15;
-    ROcp31_415 = -ROcp31_113*S15+ROcp31_414*C15;
-    ROcp31_515 = -ROcp31_213*S15+ROcp31_514*C15;
-    ROcp31_615 = -ROcp31_313*S15+ROcp31_614*C15;
-    ROcp31_116 = ROcp31_115*C16-ROcp31_714*S16;
-    ROcp31_216 = ROcp31_215*C16-ROcp31_814*S16;
-    ROcp31_316 = ROcp31_315*C16-ROcp31_914*S16;
-    ROcp31_716 = ROcp31_115*S16+ROcp31_714*C16;
-    ROcp31_816 = ROcp31_215*S16+ROcp31_814*C16;
-    ROcp31_916 = ROcp31_315*S16+ROcp31_914*C16;
-    ROcp31_417 = ROcp31_415*C17+ROcp31_716*S17;
-    ROcp31_517 = ROcp31_515*C17+ROcp31_816*S17;
-    ROcp31_617 = ROcp31_615*C17+ROcp31_916*S17;
-    ROcp31_717 = -ROcp31_415*S17+ROcp31_716*C17;
-    ROcp31_817 = -ROcp31_515*S17+ROcp31_816*C17;
-    ROcp31_917 = -ROcp31_615*S17+ROcp31_916*C17;
-    ROcp31_118 = ROcp31_116*C18-ROcp31_717*S18;
-    ROcp31_218 = ROcp31_216*C18-ROcp31_817*S18;
-    ROcp31_318 = ROcp31_316*C18-ROcp31_917*S18;
-    ROcp31_718 = ROcp31_116*S18+ROcp31_717*C18;
-    ROcp31_818 = ROcp31_216*S18+ROcp31_817*C18;
-    ROcp31_918 = ROcp31_316*S18+ROcp31_917*C18;
-    OMcp31_25 = qd[5]*C4;
-    OMcp31_35 = qd[5]*S4;
-    OMcp31_16 = qd[4]+qd[6]*S5;
-    OMcp31_26 = OMcp31_25+qd[6]*ROcp31_85;
-    OMcp31_36 = OMcp31_35+qd[6]*ROcp31_95;
-    RLcp31_17 = ROcp31_46*dpt[2][3];
-    RLcp31_27 = ROcp31_56*dpt[2][3];
-    RLcp31_37 = ROcp31_66*dpt[2][3];
-    POcp31_17 = q[1]+RLcp31_17;
-    POcp31_27 = q[2]+RLcp31_27;
-    POcp31_37 = q[3]+RLcp31_37;
-    OMcp31_17 = OMcp31_16+qd[13]*ROcp31_46;
-    OMcp31_27 = OMcp31_26+qd[13]*ROcp31_56;
-    OMcp31_37 = OMcp31_36+qd[13]*ROcp31_66;
-    ORcp31_17 = OMcp31_26*RLcp31_37-OMcp31_36*RLcp31_27;
-    ORcp31_27 = -OMcp31_16*RLcp31_37+OMcp31_36*RLcp31_17;
-    ORcp31_37 = OMcp31_16*RLcp31_27-OMcp31_26*RLcp31_17;
-    VIcp31_17 = qd[1]+ORcp31_17;
-    VIcp31_27 = qd[2]+ORcp31_27;
-    VIcp31_37 = qd[3]+ORcp31_37;
-    RLcp31_18 = ROcp31_46*dpt[2][20];
-    RLcp31_28 = ROcp31_56*dpt[2][20];
-    RLcp31_38 = ROcp31_66*dpt[2][20];
-    POcp31_18 = POcp31_17+RLcp31_18;
-    POcp31_28 = POcp31_27+RLcp31_28;
-    POcp31_38 = POcp31_37+RLcp31_38;
-    OMcp31_18 = OMcp31_17+qd[14]*ROcp31_113;
-    OMcp31_28 = OMcp31_27+qd[14]*ROcp31_213;
-    OMcp31_38 = OMcp31_37+qd[14]*ROcp31_313;
-    ORcp31_18 = OMcp31_27*RLcp31_38-OMcp31_37*RLcp31_28;
-    ORcp31_28 = -OMcp31_17*RLcp31_38+OMcp31_37*RLcp31_18;
-    ORcp31_38 = OMcp31_17*RLcp31_28-OMcp31_27*RLcp31_18;
-    VIcp31_18 = ORcp31_18+VIcp31_17;
-    VIcp31_28 = ORcp31_28+VIcp31_27;
-    VIcp31_38 = ORcp31_38+VIcp31_37;
-    RLcp31_19 = ROcp31_714*dpt[3][22];
-    RLcp31_29 = ROcp31_814*dpt[3][22];
-    RLcp31_39 = ROcp31_914*dpt[3][22];
-    POcp31_19 = POcp31_18+RLcp31_19;
-    POcp31_29 = POcp31_28+RLcp31_29;
-    POcp31_39 = POcp31_38+RLcp31_39;
-    OMcp31_19 = OMcp31_18+qd[15]*ROcp31_714;
-    OMcp31_29 = OMcp31_28+qd[15]*ROcp31_814;
-    OMcp31_39 = OMcp31_38+qd[15]*ROcp31_914;
-    ORcp31_19 = OMcp31_28*RLcp31_39-OMcp31_38*RLcp31_29;
-    ORcp31_29 = -OMcp31_18*RLcp31_39+OMcp31_38*RLcp31_19;
-    ORcp31_39 = OMcp31_18*RLcp31_29-OMcp31_28*RLcp31_19;
-    VIcp31_19 = ORcp31_19+VIcp31_18;
-    VIcp31_29 = ORcp31_29+VIcp31_28;
-    VIcp31_39 = ORcp31_39+VIcp31_38;
-    RLcp31_110 = ROcp31_714*dpt[3][25];
-    RLcp31_210 = ROcp31_814*dpt[3][25];
-    RLcp31_310 = ROcp31_914*dpt[3][25];
-    POcp31_110 = POcp31_19+RLcp31_110;
-    POcp31_210 = POcp31_29+RLcp31_210;
-    POcp31_310 = POcp31_39+RLcp31_310;
-    OMcp31_110 = OMcp31_19+qd[16]*ROcp31_415;
-    OMcp31_210 = OMcp31_29+qd[16]*ROcp31_515;
-    OMcp31_310 = OMcp31_39+qd[16]*ROcp31_615;
-    ORcp31_110 = OMcp31_29*RLcp31_310-OMcp31_39*RLcp31_210;
-    ORcp31_210 = -OMcp31_19*RLcp31_310+OMcp31_39*RLcp31_110;
-    ORcp31_310 = OMcp31_19*RLcp31_210-OMcp31_29*RLcp31_110;
-    VIcp31_110 = ORcp31_110+VIcp31_19;
-    VIcp31_210 = ORcp31_210+VIcp31_29;
-    VIcp31_310 = ORcp31_310+VIcp31_39;
-    RLcp31_111 = ROcp31_716*dpt[3][28];
-    RLcp31_211 = ROcp31_816*dpt[3][28];
-    RLcp31_311 = ROcp31_916*dpt[3][28];
-    POcp31_111 = POcp31_110+RLcp31_111;
-    POcp31_211 = POcp31_210+RLcp31_211;
-    POcp31_311 = POcp31_310+RLcp31_311;
-    OMcp31_111 = OMcp31_110+qd[17]*ROcp31_116;
-    OMcp31_211 = OMcp31_210+qd[17]*ROcp31_216;
-    OMcp31_311 = OMcp31_310+qd[17]*ROcp31_316;
-    ORcp31_111 = OMcp31_210*RLcp31_311-OMcp31_310*RLcp31_211;
-    ORcp31_211 = -OMcp31_110*RLcp31_311+OMcp31_310*RLcp31_111;
-    ORcp31_311 = OMcp31_110*RLcp31_211-OMcp31_210*RLcp31_111;
-    VIcp31_111 = ORcp31_111+VIcp31_110;
-    VIcp31_211 = ORcp31_211+VIcp31_210;
-    VIcp31_311 = ORcp31_311+VIcp31_310;
-    OMcp31_112 = OMcp31_111+qd[18]*ROcp31_417;
-    OMcp31_212 = OMcp31_211+qd[18]*ROcp31_517;
-    OMcp31_312 = OMcp31_311+qd[18]*ROcp31_617;
-    RLcp31_113 = ROcp31_718*dpt[3][33];
-    RLcp31_213 = ROcp31_818*dpt[3][33];
-    RLcp31_313 = ROcp31_918*dpt[3][33];
-    POcp31_113 = POcp31_111+RLcp31_113;
-    POcp31_213 = POcp31_211+RLcp31_213;
-    POcp31_313 = POcp31_311+RLcp31_313;
-    ORcp31_113 = OMcp31_212*RLcp31_313-OMcp31_312*RLcp31_213;
-    ORcp31_213 = -OMcp31_112*RLcp31_313+OMcp31_312*RLcp31_113;
-    ORcp31_313 = OMcp31_112*RLcp31_213-OMcp31_212*RLcp31_113;
-    VIcp31_113 = ORcp31_113+VIcp31_111;
-    VIcp31_213 = ORcp31_213+VIcp31_211;
-    VIcp31_313 = ORcp31_313+VIcp31_311;
-    PxF7[1] = POcp31_113;
-    PxF7[2] = POcp31_213;
-    PxF7[3] = POcp31_313;
-    RxF7[1][1] = ROcp31_118;
-    RxF7[1][2] = ROcp31_218;
-    RxF7[1][3] = ROcp31_318;
-    RxF7[2][1] = ROcp31_417;
-    RxF7[2][2] = ROcp31_517;
-    RxF7[2][3] = ROcp31_617;
-    RxF7[3][1] = ROcp31_718;
-    RxF7[3][2] = ROcp31_818;
-    RxF7[3][3] = ROcp31_918;
-    VxF7[1] = VIcp31_113;
-    VxF7[2] = VIcp31_213;
-    VxF7[3] = VIcp31_313;
-    OMxF7[1] = OMcp31_112;
-    OMxF7[2] = OMcp31_212;
-    OMxF7[3] = OMcp31_312;
-    ROcp32_25 = S4*S5;
-    ROcp32_35 = -C4*S5;
-    ROcp32_85 = -S4*C5;
-    ROcp32_95 = C4*C5;
-    ROcp32_16 = C5*C6;
-    ROcp32_26 = ROcp32_25*C6+C4*S6;
-    ROcp32_36 = ROcp32_35*C6+S4*S6;
-    ROcp32_46 = -C5*S6;
-    ROcp32_56 = -ROcp32_25*S6+C4*C6;
-    ROcp32_66 = -ROcp32_35*S6+S4*C6;
-    ROcp32_419 = ROcp32_46*C19+S19*S5;
-    ROcp32_519 = ROcp32_56*C19+ROcp32_85*S19;
-    ROcp32_619 = ROcp32_66*C19+ROcp32_95*S19;
-    ROcp32_719 = -ROcp32_46*S19+C19*S5;
-    ROcp32_819 = -ROcp32_56*S19+ROcp32_85*C19;
-    ROcp32_919 = -ROcp32_66*S19+ROcp32_95*C19;
-    ROcp32_120 = ROcp32_16*C20-ROcp32_719*S20;
-    ROcp32_220 = ROcp32_26*C20-ROcp32_819*S20;
-    ROcp32_320 = ROcp32_36*C20-ROcp32_919*S20;
-    ROcp32_720 = ROcp32_16*S20+ROcp32_719*C20;
-    ROcp32_820 = ROcp32_26*S20+ROcp32_819*C20;
-    ROcp32_920 = ROcp32_36*S20+ROcp32_919*C20;
-    ROcp32_121 = ROcp32_120*C21+ROcp32_419*S21;
-    ROcp32_221 = ROcp32_220*C21+ROcp32_519*S21;
-    ROcp32_321 = ROcp32_320*C21+ROcp32_619*S21;
-    ROcp32_421 = -ROcp32_120*S21+ROcp32_419*C21;
-    ROcp32_521 = -ROcp32_220*S21+ROcp32_519*C21;
-    ROcp32_621 = -ROcp32_320*S21+ROcp32_619*C21;
-    OMcp32_25 = qd[5]*C4;
-    OMcp32_35 = qd[5]*S4;
-    OMcp32_16 = qd[4]+qd[6]*S5;
-    OMcp32_26 = OMcp32_25+qd[6]*ROcp32_85;
-    OMcp32_36 = OMcp32_35+qd[6]*ROcp32_95;
-    RLcp32_17 = ROcp32_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp32_27 = ROcp32_26*dpt[1][4]+ROcp32_85*dpt[3][4];
-    RLcp32_37 = ROcp32_36*dpt[1][4]+ROcp32_95*dpt[3][4];
-    POcp32_17 = q[1]+RLcp32_17;
-    POcp32_27 = q[2]+RLcp32_27;
-    POcp32_37 = q[3]+RLcp32_37;
-    OMcp32_17 = OMcp32_16+qd[19]*ROcp32_16;
-    OMcp32_27 = OMcp32_26+qd[19]*ROcp32_26;
-    OMcp32_37 = OMcp32_36+qd[19]*ROcp32_36;
-    ORcp32_17 = OMcp32_26*RLcp32_37-OMcp32_36*RLcp32_27;
-    ORcp32_27 = -OMcp32_16*RLcp32_37+OMcp32_36*RLcp32_17;
-    ORcp32_37 = OMcp32_16*RLcp32_27-OMcp32_26*RLcp32_17;
-    VIcp32_17 = qd[1]+ORcp32_17;
-    VIcp32_27 = qd[2]+ORcp32_27;
-    VIcp32_37 = qd[3]+ORcp32_37;
-    OMcp32_18 = OMcp32_17+qd[20]*ROcp32_419;
-    OMcp32_28 = OMcp32_27+qd[20]*ROcp32_519;
-    OMcp32_38 = OMcp32_37+qd[20]*ROcp32_619;
-    RLcp32_19 = ROcp32_720*dpt[3][36];
-    RLcp32_29 = ROcp32_820*dpt[3][36];
-    RLcp32_39 = ROcp32_920*dpt[3][36];
-    POcp32_19 = POcp32_17+RLcp32_19;
-    POcp32_29 = POcp32_27+RLcp32_29;
-    POcp32_39 = POcp32_37+RLcp32_39;
-    OMcp32_19 = OMcp32_18+qd[21]*ROcp32_720;
-    OMcp32_29 = OMcp32_28+qd[21]*ROcp32_820;
-    OMcp32_39 = OMcp32_38+qd[21]*ROcp32_920;
-    ORcp32_19 = OMcp32_28*RLcp32_39-OMcp32_38*RLcp32_29;
-    ORcp32_29 = -OMcp32_18*RLcp32_39+OMcp32_38*RLcp32_19;
-    ORcp32_39 = OMcp32_18*RLcp32_29-OMcp32_28*RLcp32_19;
-    VIcp32_19 = ORcp32_19+VIcp32_17;
-    VIcp32_29 = ORcp32_29+VIcp32_27;
-    VIcp32_39 = ORcp32_39+VIcp32_37;
-    PxF8[1] = POcp32_19;
-    PxF8[2] = POcp32_29;
-    PxF8[3] = POcp32_39;
-    RxF8[1][1] = ROcp32_121;
-    RxF8[1][2] = ROcp32_221;
-    RxF8[1][3] = ROcp32_321;
-    RxF8[2][1] = ROcp32_421;
-    RxF8[2][2] = ROcp32_521;
-    RxF8[2][3] = ROcp32_621;
-    RxF8[3][1] = ROcp32_720;
-    RxF8[3][2] = ROcp32_820;
-    RxF8[3][3] = ROcp32_920;
-    VxF8[1] = VIcp32_19;
-    VxF8[2] = VIcp32_29;
-    VxF8[3] = VIcp32_39;
-    OMxF8[1] = OMcp32_19;
-    OMxF8[2] = OMcp32_29;
-    OMxF8[3] = OMcp32_39;
-    ROcp33_25 = S4*S5;
-    ROcp33_35 = -C4*S5;
-    ROcp33_85 = -S4*C5;
-    ROcp33_95 = C4*C5;
-    ROcp33_16 = C5*C6;
-    ROcp33_26 = ROcp33_25*C6+C4*S6;
-    ROcp33_36 = ROcp33_35*C6+S4*S6;
-    ROcp33_46 = -C5*S6;
-    ROcp33_56 = -ROcp33_25*S6+C4*C6;
-    ROcp33_66 = -ROcp33_35*S6+S4*C6;
-    ROcp33_419 = ROcp33_46*C19+S19*S5;
-    ROcp33_519 = ROcp33_56*C19+ROcp33_85*S19;
-    ROcp33_619 = ROcp33_66*C19+ROcp33_95*S19;
-    ROcp33_719 = -ROcp33_46*S19+C19*S5;
-    ROcp33_819 = -ROcp33_56*S19+ROcp33_85*C19;
-    ROcp33_919 = -ROcp33_66*S19+ROcp33_95*C19;
-    ROcp33_120 = ROcp33_16*C20-ROcp33_719*S20;
-    ROcp33_220 = ROcp33_26*C20-ROcp33_819*S20;
-    ROcp33_320 = ROcp33_36*C20-ROcp33_919*S20;
-    ROcp33_720 = ROcp33_16*S20+ROcp33_719*C20;
-    ROcp33_820 = ROcp33_26*S20+ROcp33_819*C20;
-    ROcp33_920 = ROcp33_36*S20+ROcp33_919*C20;
-    ROcp33_121 = ROcp33_120*C21+ROcp33_419*S21;
-    ROcp33_221 = ROcp33_220*C21+ROcp33_519*S21;
-    ROcp33_321 = ROcp33_320*C21+ROcp33_619*S21;
-    ROcp33_421 = -ROcp33_120*S21+ROcp33_419*C21;
-    ROcp33_521 = -ROcp33_220*S21+ROcp33_519*C21;
-    ROcp33_621 = -ROcp33_320*S21+ROcp33_619*C21;
-    ROcp33_122 = ROcp33_121*C22-ROcp33_720*S22;
-    ROcp33_222 = ROcp33_221*C22-ROcp33_820*S22;
-    ROcp33_322 = ROcp33_321*C22-ROcp33_920*S22;
-    ROcp33_722 = ROcp33_121*S22+ROcp33_720*C22;
-    ROcp33_822 = ROcp33_221*S22+ROcp33_820*C22;
-    ROcp33_922 = ROcp33_321*S22+ROcp33_920*C22;
-    ROcp33_423 = ROcp33_421*C23+ROcp33_722*S23;
-    ROcp33_523 = ROcp33_521*C23+ROcp33_822*S23;
-    ROcp33_623 = ROcp33_621*C23+ROcp33_922*S23;
-    ROcp33_723 = -ROcp33_421*S23+ROcp33_722*C23;
-    ROcp33_823 = -ROcp33_521*S23+ROcp33_822*C23;
-    ROcp33_923 = -ROcp33_621*S23+ROcp33_922*C23;
-    ROcp33_124 = ROcp33_122*C24+ROcp33_423*S24;
-    ROcp33_224 = ROcp33_222*C24+ROcp33_523*S24;
-    ROcp33_324 = ROcp33_322*C24+ROcp33_623*S24;
-    ROcp33_424 = -ROcp33_122*S24+ROcp33_423*C24;
-    ROcp33_524 = -ROcp33_222*S24+ROcp33_523*C24;
-    ROcp33_624 = -ROcp33_322*S24+ROcp33_623*C24;
-    OMcp33_25 = qd[5]*C4;
-    OMcp33_35 = qd[5]*S4;
-    OMcp33_16 = qd[4]+qd[6]*S5;
-    OMcp33_26 = OMcp33_25+qd[6]*ROcp33_85;
-    OMcp33_36 = OMcp33_35+qd[6]*ROcp33_95;
-    RLcp33_17 = ROcp33_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp33_27 = ROcp33_26*dpt[1][4]+ROcp33_85*dpt[3][4];
-    RLcp33_37 = ROcp33_36*dpt[1][4]+ROcp33_95*dpt[3][4];
-    POcp33_17 = q[1]+RLcp33_17;
-    POcp33_27 = q[2]+RLcp33_27;
-    POcp33_37 = q[3]+RLcp33_37;
-    OMcp33_17 = OMcp33_16+qd[19]*ROcp33_16;
-    OMcp33_27 = OMcp33_26+qd[19]*ROcp33_26;
-    OMcp33_37 = OMcp33_36+qd[19]*ROcp33_36;
-    ORcp33_17 = OMcp33_26*RLcp33_37-OMcp33_36*RLcp33_27;
-    ORcp33_27 = -OMcp33_16*RLcp33_37+OMcp33_36*RLcp33_17;
-    ORcp33_37 = OMcp33_16*RLcp33_27-OMcp33_26*RLcp33_17;
-    VIcp33_17 = qd[1]+ORcp33_17;
-    VIcp33_27 = qd[2]+ORcp33_27;
-    VIcp33_37 = qd[3]+ORcp33_37;
-    OMcp33_18 = OMcp33_17+qd[20]*ROcp33_419;
-    OMcp33_28 = OMcp33_27+qd[20]*ROcp33_519;
-    OMcp33_38 = OMcp33_37+qd[20]*ROcp33_619;
-    RLcp33_19 = ROcp33_720*dpt[3][36];
-    RLcp33_29 = ROcp33_820*dpt[3][36];
-    RLcp33_39 = ROcp33_920*dpt[3][36];
-    POcp33_19 = POcp33_17+RLcp33_19;
-    POcp33_29 = POcp33_27+RLcp33_29;
-    POcp33_39 = POcp33_37+RLcp33_39;
-    OMcp33_19 = OMcp33_18+qd[21]*ROcp33_720;
-    OMcp33_29 = OMcp33_28+qd[21]*ROcp33_820;
-    OMcp33_39 = OMcp33_38+qd[21]*ROcp33_920;
-    ORcp33_19 = OMcp33_28*RLcp33_39-OMcp33_38*RLcp33_29;
-    ORcp33_29 = -OMcp33_18*RLcp33_39+OMcp33_38*RLcp33_19;
-    ORcp33_39 = OMcp33_18*RLcp33_29-OMcp33_28*RLcp33_19;
-    VIcp33_19 = ORcp33_19+VIcp33_17;
-    VIcp33_29 = ORcp33_29+VIcp33_27;
-    VIcp33_39 = ORcp33_39+VIcp33_37;
-    RLcp33_110 = ROcp33_121*dpt[1][40]+ROcp33_421*dpt[2][40]+ROcp33_720*dpt[3][40];
-    RLcp33_210 = ROcp33_221*dpt[1][40]+ROcp33_521*dpt[2][40]+ROcp33_820*dpt[3][40];
-    RLcp33_310 = ROcp33_321*dpt[1][40]+ROcp33_621*dpt[2][40]+ROcp33_920*dpt[3][40];
-    POcp33_110 = POcp33_19+RLcp33_110;
-    POcp33_210 = POcp33_29+RLcp33_210;
-    POcp33_310 = POcp33_39+RLcp33_310;
-    OMcp33_110 = OMcp33_19+qd[22]*ROcp33_421;
-    OMcp33_210 = OMcp33_29+qd[22]*ROcp33_521;
-    OMcp33_310 = OMcp33_39+qd[22]*ROcp33_621;
-    ORcp33_110 = OMcp33_29*RLcp33_310-OMcp33_39*RLcp33_210;
-    ORcp33_210 = -OMcp33_19*RLcp33_310+OMcp33_39*RLcp33_110;
-    ORcp33_310 = OMcp33_19*RLcp33_210-OMcp33_29*RLcp33_110;
-    VIcp33_110 = ORcp33_110+VIcp33_19;
-    VIcp33_210 = ORcp33_210+VIcp33_29;
-    VIcp33_310 = ORcp33_310+VIcp33_39;
-    RLcp33_111 = ROcp33_421*dpt[2][43];
-    RLcp33_211 = ROcp33_521*dpt[2][43];
-    RLcp33_311 = ROcp33_621*dpt[2][43];
-    POcp33_111 = POcp33_110+RLcp33_111;
-    POcp33_211 = POcp33_210+RLcp33_211;
-    POcp33_311 = POcp33_310+RLcp33_311;
-    OMcp33_111 = OMcp33_110+qd[23]*ROcp33_122;
-    OMcp33_211 = OMcp33_210+qd[23]*ROcp33_222;
-    OMcp33_311 = OMcp33_310+qd[23]*ROcp33_322;
-    ORcp33_111 = OMcp33_210*RLcp33_311-OMcp33_310*RLcp33_211;
-    ORcp33_211 = -OMcp33_110*RLcp33_311+OMcp33_310*RLcp33_111;
-    ORcp33_311 = OMcp33_110*RLcp33_211-OMcp33_210*RLcp33_111;
-    VIcp33_111 = ORcp33_111+VIcp33_110;
-    VIcp33_211 = ORcp33_211+VIcp33_210;
-    VIcp33_311 = ORcp33_311+VIcp33_310;
-    RLcp33_112 = ROcp33_723*dpt[3][45];
-    RLcp33_212 = ROcp33_823*dpt[3][45];
-    RLcp33_312 = ROcp33_923*dpt[3][45];
-    POcp33_112 = POcp33_111+RLcp33_112;
-    POcp33_212 = POcp33_211+RLcp33_212;
-    POcp33_312 = POcp33_311+RLcp33_312;
-    OMcp33_112 = OMcp33_111+qd[24]*ROcp33_723;
-    OMcp33_212 = OMcp33_211+qd[24]*ROcp33_823;
-    OMcp33_312 = OMcp33_311+qd[24]*ROcp33_923;
-    ORcp33_112 = OMcp33_211*RLcp33_312-OMcp33_311*RLcp33_212;
-    ORcp33_212 = -OMcp33_111*RLcp33_312+OMcp33_311*RLcp33_112;
-    ORcp33_312 = OMcp33_111*RLcp33_212-OMcp33_211*RLcp33_112;
-    VIcp33_112 = ORcp33_112+VIcp33_111;
-    VIcp33_212 = ORcp33_212+VIcp33_211;
-    VIcp33_312 = ORcp33_312+VIcp33_311;
-    PxF9[1] = POcp33_112;
-    PxF9[2] = POcp33_212;
-    PxF9[3] = POcp33_312;
-    RxF9[1][1] = ROcp33_124;
-    RxF9[1][2] = ROcp33_224;
-    RxF9[1][3] = ROcp33_324;
-    RxF9[2][1] = ROcp33_424;
-    RxF9[2][2] = ROcp33_524;
-    RxF9[2][3] = ROcp33_624;
-    RxF9[3][1] = ROcp33_723;
-    RxF9[3][2] = ROcp33_823;
-    RxF9[3][3] = ROcp33_923;
-    VxF9[1] = VIcp33_112;
-    VxF9[2] = VIcp33_212;
-    VxF9[3] = VIcp33_312;
-    OMxF9[1] = OMcp33_112;
-    OMxF9[2] = OMcp33_212;
-    OMxF9[3] = OMcp33_312;
-    ROcp34_25 = S4*S5;
-    ROcp34_35 = -C4*S5;
-    ROcp34_85 = -S4*C5;
-    ROcp34_95 = C4*C5;
-    ROcp34_16 = C5*C6;
-    ROcp34_26 = ROcp34_25*C6+C4*S6;
-    ROcp34_36 = ROcp34_35*C6+S4*S6;
-    ROcp34_46 = -C5*S6;
-    ROcp34_56 = -ROcp34_25*S6+C4*C6;
-    ROcp34_66 = -ROcp34_35*S6+S4*C6;
-    ROcp34_419 = ROcp34_46*C19+S19*S5;
-    ROcp34_519 = ROcp34_56*C19+ROcp34_85*S19;
-    ROcp34_619 = ROcp34_66*C19+ROcp34_95*S19;
-    ROcp34_719 = -ROcp34_46*S19+C19*S5;
-    ROcp34_819 = -ROcp34_56*S19+ROcp34_85*C19;
-    ROcp34_919 = -ROcp34_66*S19+ROcp34_95*C19;
-    ROcp34_120 = ROcp34_16*C20-ROcp34_719*S20;
-    ROcp34_220 = ROcp34_26*C20-ROcp34_819*S20;
-    ROcp34_320 = ROcp34_36*C20-ROcp34_919*S20;
-    ROcp34_720 = ROcp34_16*S20+ROcp34_719*C20;
-    ROcp34_820 = ROcp34_26*S20+ROcp34_819*C20;
-    ROcp34_920 = ROcp34_36*S20+ROcp34_919*C20;
-    ROcp34_121 = ROcp34_120*C21+ROcp34_419*S21;
-    ROcp34_221 = ROcp34_220*C21+ROcp34_519*S21;
-    ROcp34_321 = ROcp34_320*C21+ROcp34_619*S21;
-    ROcp34_421 = -ROcp34_120*S21+ROcp34_419*C21;
-    ROcp34_521 = -ROcp34_220*S21+ROcp34_519*C21;
-    ROcp34_621 = -ROcp34_320*S21+ROcp34_619*C21;
-    ROcp34_122 = ROcp34_121*C22-ROcp34_720*S22;
-    ROcp34_222 = ROcp34_221*C22-ROcp34_820*S22;
-    ROcp34_322 = ROcp34_321*C22-ROcp34_920*S22;
-    ROcp34_722 = ROcp34_121*S22+ROcp34_720*C22;
-    ROcp34_822 = ROcp34_221*S22+ROcp34_820*C22;
-    ROcp34_922 = ROcp34_321*S22+ROcp34_920*C22;
-    ROcp34_423 = ROcp34_421*C23+ROcp34_722*S23;
-    ROcp34_523 = ROcp34_521*C23+ROcp34_822*S23;
-    ROcp34_623 = ROcp34_621*C23+ROcp34_922*S23;
-    ROcp34_723 = -ROcp34_421*S23+ROcp34_722*C23;
-    ROcp34_823 = -ROcp34_521*S23+ROcp34_822*C23;
-    ROcp34_923 = -ROcp34_621*S23+ROcp34_922*C23;
-    ROcp34_124 = ROcp34_122*C24+ROcp34_423*S24;
-    ROcp34_224 = ROcp34_222*C24+ROcp34_523*S24;
-    ROcp34_324 = ROcp34_322*C24+ROcp34_623*S24;
-    ROcp34_424 = -ROcp34_122*S24+ROcp34_423*C24;
-    ROcp34_524 = -ROcp34_222*S24+ROcp34_523*C24;
-    ROcp34_624 = -ROcp34_322*S24+ROcp34_623*C24;
-    ROcp34_125 = ROcp34_124*C25-ROcp34_723*S25;
-    ROcp34_225 = ROcp34_224*C25-ROcp34_823*S25;
-    ROcp34_325 = ROcp34_324*C25-ROcp34_923*S25;
-    ROcp34_725 = ROcp34_124*S25+ROcp34_723*C25;
-    ROcp34_825 = ROcp34_224*S25+ROcp34_823*C25;
-    ROcp34_925 = ROcp34_324*S25+ROcp34_923*C25;
-    OMcp34_25 = qd[5]*C4;
-    OMcp34_35 = qd[5]*S4;
-    OMcp34_16 = qd[4]+qd[6]*S5;
-    OMcp34_26 = OMcp34_25+qd[6]*ROcp34_85;
-    OMcp34_36 = OMcp34_35+qd[6]*ROcp34_95;
-    RLcp34_17 = ROcp34_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp34_27 = ROcp34_26*dpt[1][4]+ROcp34_85*dpt[3][4];
-    RLcp34_37 = ROcp34_36*dpt[1][4]+ROcp34_95*dpt[3][4];
-    POcp34_17 = q[1]+RLcp34_17;
-    POcp34_27 = q[2]+RLcp34_27;
-    POcp34_37 = q[3]+RLcp34_37;
-    OMcp34_17 = OMcp34_16+qd[19]*ROcp34_16;
-    OMcp34_27 = OMcp34_26+qd[19]*ROcp34_26;
-    OMcp34_37 = OMcp34_36+qd[19]*ROcp34_36;
-    ORcp34_17 = OMcp34_26*RLcp34_37-OMcp34_36*RLcp34_27;
-    ORcp34_27 = -OMcp34_16*RLcp34_37+OMcp34_36*RLcp34_17;
-    ORcp34_37 = OMcp34_16*RLcp34_27-OMcp34_26*RLcp34_17;
-    VIcp34_17 = qd[1]+ORcp34_17;
-    VIcp34_27 = qd[2]+ORcp34_27;
-    VIcp34_37 = qd[3]+ORcp34_37;
-    OMcp34_18 = OMcp34_17+qd[20]*ROcp34_419;
-    OMcp34_28 = OMcp34_27+qd[20]*ROcp34_519;
-    OMcp34_38 = OMcp34_37+qd[20]*ROcp34_619;
-    RLcp34_19 = ROcp34_720*dpt[3][36];
-    RLcp34_29 = ROcp34_820*dpt[3][36];
-    RLcp34_39 = ROcp34_920*dpt[3][36];
-    POcp34_19 = POcp34_17+RLcp34_19;
-    POcp34_29 = POcp34_27+RLcp34_29;
-    POcp34_39 = POcp34_37+RLcp34_39;
-    OMcp34_19 = OMcp34_18+qd[21]*ROcp34_720;
-    OMcp34_29 = OMcp34_28+qd[21]*ROcp34_820;
-    OMcp34_39 = OMcp34_38+qd[21]*ROcp34_920;
-    ORcp34_19 = OMcp34_28*RLcp34_39-OMcp34_38*RLcp34_29;
-    ORcp34_29 = -OMcp34_18*RLcp34_39+OMcp34_38*RLcp34_19;
-    ORcp34_39 = OMcp34_18*RLcp34_29-OMcp34_28*RLcp34_19;
-    VIcp34_19 = ORcp34_19+VIcp34_17;
-    VIcp34_29 = ORcp34_29+VIcp34_27;
-    VIcp34_39 = ORcp34_39+VIcp34_37;
-    RLcp34_110 = ROcp34_121*dpt[1][40]+ROcp34_421*dpt[2][40]+ROcp34_720*dpt[3][40];
-    RLcp34_210 = ROcp34_221*dpt[1][40]+ROcp34_521*dpt[2][40]+ROcp34_820*dpt[3][40];
-    RLcp34_310 = ROcp34_321*dpt[1][40]+ROcp34_621*dpt[2][40]+ROcp34_920*dpt[3][40];
-    POcp34_110 = POcp34_19+RLcp34_110;
-    POcp34_210 = POcp34_29+RLcp34_210;
-    POcp34_310 = POcp34_39+RLcp34_310;
-    OMcp34_110 = OMcp34_19+qd[22]*ROcp34_421;
-    OMcp34_210 = OMcp34_29+qd[22]*ROcp34_521;
-    OMcp34_310 = OMcp34_39+qd[22]*ROcp34_621;
-    ORcp34_110 = OMcp34_29*RLcp34_310-OMcp34_39*RLcp34_210;
-    ORcp34_210 = -OMcp34_19*RLcp34_310+OMcp34_39*RLcp34_110;
-    ORcp34_310 = OMcp34_19*RLcp34_210-OMcp34_29*RLcp34_110;
-    VIcp34_110 = ORcp34_110+VIcp34_19;
-    VIcp34_210 = ORcp34_210+VIcp34_29;
-    VIcp34_310 = ORcp34_310+VIcp34_39;
-    RLcp34_111 = ROcp34_421*dpt[2][43];
-    RLcp34_211 = ROcp34_521*dpt[2][43];
-    RLcp34_311 = ROcp34_621*dpt[2][43];
-    POcp34_111 = POcp34_110+RLcp34_111;
-    POcp34_211 = POcp34_210+RLcp34_211;
-    POcp34_311 = POcp34_310+RLcp34_311;
-    OMcp34_111 = OMcp34_110+qd[23]*ROcp34_122;
-    OMcp34_211 = OMcp34_210+qd[23]*ROcp34_222;
-    OMcp34_311 = OMcp34_310+qd[23]*ROcp34_322;
-    ORcp34_111 = OMcp34_210*RLcp34_311-OMcp34_310*RLcp34_211;
-    ORcp34_211 = -OMcp34_110*RLcp34_311+OMcp34_310*RLcp34_111;
-    ORcp34_311 = OMcp34_110*RLcp34_211-OMcp34_210*RLcp34_111;
-    VIcp34_111 = ORcp34_111+VIcp34_110;
-    VIcp34_211 = ORcp34_211+VIcp34_210;
-    VIcp34_311 = ORcp34_311+VIcp34_310;
-    RLcp34_112 = ROcp34_723*dpt[3][45];
-    RLcp34_212 = ROcp34_823*dpt[3][45];
-    RLcp34_312 = ROcp34_923*dpt[3][45];
-    POcp34_112 = POcp34_111+RLcp34_112;
-    POcp34_212 = POcp34_211+RLcp34_212;
-    POcp34_312 = POcp34_311+RLcp34_312;
-    OMcp34_112 = OMcp34_111+qd[24]*ROcp34_723;
-    OMcp34_212 = OMcp34_211+qd[24]*ROcp34_823;
-    OMcp34_312 = OMcp34_311+qd[24]*ROcp34_923;
-    ORcp34_112 = OMcp34_211*RLcp34_312-OMcp34_311*RLcp34_212;
-    ORcp34_212 = -OMcp34_111*RLcp34_312+OMcp34_311*RLcp34_112;
-    ORcp34_312 = OMcp34_111*RLcp34_212-OMcp34_211*RLcp34_112;
-    VIcp34_112 = ORcp34_112+VIcp34_111;
-    VIcp34_212 = ORcp34_212+VIcp34_211;
-    VIcp34_312 = ORcp34_312+VIcp34_311;
-    RLcp34_113 = ROcp34_723*dpt[3][48];
-    RLcp34_213 = ROcp34_823*dpt[3][48];
-    RLcp34_313 = ROcp34_923*dpt[3][48];
-    POcp34_113 = POcp34_112+RLcp34_113;
-    POcp34_213 = POcp34_212+RLcp34_213;
-    POcp34_313 = POcp34_312+RLcp34_313;
-    OMcp34_113 = OMcp34_112+qd[25]*ROcp34_424;
-    OMcp34_213 = OMcp34_212+qd[25]*ROcp34_524;
-    OMcp34_313 = OMcp34_312+qd[25]*ROcp34_624;
-    ORcp34_113 = OMcp34_212*RLcp34_313-OMcp34_312*RLcp34_213;
-    ORcp34_213 = -OMcp34_112*RLcp34_313+OMcp34_312*RLcp34_113;
-    ORcp34_313 = OMcp34_112*RLcp34_213-OMcp34_212*RLcp34_113;
-    VIcp34_113 = ORcp34_113+VIcp34_112;
-    VIcp34_213 = ORcp34_213+VIcp34_212;
-    VIcp34_313 = ORcp34_313+VIcp34_312;
-    PxF10[1] = POcp34_113;
-    PxF10[2] = POcp34_213;
-    PxF10[3] = POcp34_313;
-    RxF10[1][1] = ROcp34_125;
-    RxF10[1][2] = ROcp34_225;
-    RxF10[1][3] = ROcp34_325;
-    RxF10[2][1] = ROcp34_424;
-    RxF10[2][2] = ROcp34_524;
-    RxF10[2][3] = ROcp34_624;
-    RxF10[3][1] = ROcp34_725;
-    RxF10[3][2] = ROcp34_825;
-    RxF10[3][3] = ROcp34_925;
-    VxF10[1] = VIcp34_113;
-    VxF10[2] = VIcp34_213;
-    VxF10[3] = VIcp34_313;
-    OMxF10[1] = OMcp34_113;
-    OMxF10[2] = OMcp34_213;
-    OMxF10[3] = OMcp34_313;
-    ROcp35_25 = S4*S5;
-    ROcp35_35 = -C4*S5;
-    ROcp35_85 = -S4*C5;
-    ROcp35_95 = C4*C5;
-    ROcp35_16 = C5*C6;
-    ROcp35_26 = ROcp35_25*C6+C4*S6;
-    ROcp35_36 = ROcp35_35*C6+S4*S6;
-    ROcp35_46 = -C5*S6;
-    ROcp35_56 = -ROcp35_25*S6+C4*C6;
-    ROcp35_66 = -ROcp35_35*S6+S4*C6;
-    ROcp35_419 = ROcp35_46*C19+S19*S5;
-    ROcp35_519 = ROcp35_56*C19+ROcp35_85*S19;
-    ROcp35_619 = ROcp35_66*C19+ROcp35_95*S19;
-    ROcp35_719 = -ROcp35_46*S19+C19*S5;
-    ROcp35_819 = -ROcp35_56*S19+ROcp35_85*C19;
-    ROcp35_919 = -ROcp35_66*S19+ROcp35_95*C19;
-    ROcp35_120 = ROcp35_16*C20-ROcp35_719*S20;
-    ROcp35_220 = ROcp35_26*C20-ROcp35_819*S20;
-    ROcp35_320 = ROcp35_36*C20-ROcp35_919*S20;
-    ROcp35_720 = ROcp35_16*S20+ROcp35_719*C20;
-    ROcp35_820 = ROcp35_26*S20+ROcp35_819*C20;
-    ROcp35_920 = ROcp35_36*S20+ROcp35_919*C20;
-    ROcp35_121 = ROcp35_120*C21+ROcp35_419*S21;
-    ROcp35_221 = ROcp35_220*C21+ROcp35_519*S21;
-    ROcp35_321 = ROcp35_320*C21+ROcp35_619*S21;
-    ROcp35_421 = -ROcp35_120*S21+ROcp35_419*C21;
-    ROcp35_521 = -ROcp35_220*S21+ROcp35_519*C21;
-    ROcp35_621 = -ROcp35_320*S21+ROcp35_619*C21;
-    ROcp35_126 = ROcp35_121*C26-ROcp35_720*S26;
-    ROcp35_226 = ROcp35_221*C26-ROcp35_820*S26;
-    ROcp35_326 = ROcp35_321*C26-ROcp35_920*S26;
-    ROcp35_726 = ROcp35_121*S26+ROcp35_720*C26;
-    ROcp35_826 = ROcp35_221*S26+ROcp35_820*C26;
-    ROcp35_926 = ROcp35_321*S26+ROcp35_920*C26;
-    ROcp35_427 = ROcp35_421*C27+ROcp35_726*S27;
-    ROcp35_527 = ROcp35_521*C27+ROcp35_826*S27;
-    ROcp35_627 = ROcp35_621*C27+ROcp35_926*S27;
-    ROcp35_727 = -ROcp35_421*S27+ROcp35_726*C27;
-    ROcp35_827 = -ROcp35_521*S27+ROcp35_826*C27;
-    ROcp35_927 = -ROcp35_621*S27+ROcp35_926*C27;
-    ROcp35_128 = ROcp35_126*C28+ROcp35_427*S28;
-    ROcp35_228 = ROcp35_226*C28+ROcp35_527*S28;
-    ROcp35_328 = ROcp35_326*C28+ROcp35_627*S28;
-    ROcp35_428 = -ROcp35_126*S28+ROcp35_427*C28;
-    ROcp35_528 = -ROcp35_226*S28+ROcp35_527*C28;
-    ROcp35_628 = -ROcp35_326*S28+ROcp35_627*C28;
-    OMcp35_25 = qd[5]*C4;
-    OMcp35_35 = qd[5]*S4;
-    OMcp35_16 = qd[4]+qd[6]*S5;
-    OMcp35_26 = OMcp35_25+qd[6]*ROcp35_85;
-    OMcp35_36 = OMcp35_35+qd[6]*ROcp35_95;
-    RLcp35_17 = ROcp35_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp35_27 = ROcp35_26*dpt[1][4]+ROcp35_85*dpt[3][4];
-    RLcp35_37 = ROcp35_36*dpt[1][4]+ROcp35_95*dpt[3][4];
-    POcp35_17 = q[1]+RLcp35_17;
-    POcp35_27 = q[2]+RLcp35_27;
-    POcp35_37 = q[3]+RLcp35_37;
-    OMcp35_17 = OMcp35_16+qd[19]*ROcp35_16;
-    OMcp35_27 = OMcp35_26+qd[19]*ROcp35_26;
-    OMcp35_37 = OMcp35_36+qd[19]*ROcp35_36;
-    ORcp35_17 = OMcp35_26*RLcp35_37-OMcp35_36*RLcp35_27;
-    ORcp35_27 = -OMcp35_16*RLcp35_37+OMcp35_36*RLcp35_17;
-    ORcp35_37 = OMcp35_16*RLcp35_27-OMcp35_26*RLcp35_17;
-    VIcp35_17 = qd[1]+ORcp35_17;
-    VIcp35_27 = qd[2]+ORcp35_27;
-    VIcp35_37 = qd[3]+ORcp35_37;
-    OMcp35_18 = OMcp35_17+qd[20]*ROcp35_419;
-    OMcp35_28 = OMcp35_27+qd[20]*ROcp35_519;
-    OMcp35_38 = OMcp35_37+qd[20]*ROcp35_619;
-    RLcp35_19 = ROcp35_720*dpt[3][36];
-    RLcp35_29 = ROcp35_820*dpt[3][36];
-    RLcp35_39 = ROcp35_920*dpt[3][36];
-    POcp35_19 = POcp35_17+RLcp35_19;
-    POcp35_29 = POcp35_27+RLcp35_29;
-    POcp35_39 = POcp35_37+RLcp35_39;
-    OMcp35_19 = OMcp35_18+qd[21]*ROcp35_720;
-    OMcp35_29 = OMcp35_28+qd[21]*ROcp35_820;
-    OMcp35_39 = OMcp35_38+qd[21]*ROcp35_920;
-    ORcp35_19 = OMcp35_28*RLcp35_39-OMcp35_38*RLcp35_29;
-    ORcp35_29 = -OMcp35_18*RLcp35_39+OMcp35_38*RLcp35_19;
-    ORcp35_39 = OMcp35_18*RLcp35_29-OMcp35_28*RLcp35_19;
-    VIcp35_19 = ORcp35_19+VIcp35_17;
-    VIcp35_29 = ORcp35_29+VIcp35_27;
-    VIcp35_39 = ORcp35_39+VIcp35_37;
-    RLcp35_110 = ROcp35_121*dpt[1][41]+ROcp35_421*dpt[2][41]+ROcp35_720*dpt[3][41];
-    RLcp35_210 = ROcp35_221*dpt[1][41]+ROcp35_521*dpt[2][41]+ROcp35_820*dpt[3][41];
-    RLcp35_310 = ROcp35_321*dpt[1][41]+ROcp35_621*dpt[2][41]+ROcp35_920*dpt[3][41];
-    POcp35_110 = POcp35_19+RLcp35_110;
-    POcp35_210 = POcp35_29+RLcp35_210;
-    POcp35_310 = POcp35_39+RLcp35_310;
-    OMcp35_110 = OMcp35_19+qd[26]*ROcp35_421;
-    OMcp35_210 = OMcp35_29+qd[26]*ROcp35_521;
-    OMcp35_310 = OMcp35_39+qd[26]*ROcp35_621;
-    ORcp35_110 = OMcp35_29*RLcp35_310-OMcp35_39*RLcp35_210;
-    ORcp35_210 = -OMcp35_19*RLcp35_310+OMcp35_39*RLcp35_110;
-    ORcp35_310 = OMcp35_19*RLcp35_210-OMcp35_29*RLcp35_110;
-    VIcp35_110 = ORcp35_110+VIcp35_19;
-    VIcp35_210 = ORcp35_210+VIcp35_29;
-    VIcp35_310 = ORcp35_310+VIcp35_39;
-    RLcp35_111 = ROcp35_421*dpt[2][52];
-    RLcp35_211 = ROcp35_521*dpt[2][52];
-    RLcp35_311 = ROcp35_621*dpt[2][52];
-    POcp35_111 = POcp35_110+RLcp35_111;
-    POcp35_211 = POcp35_210+RLcp35_211;
-    POcp35_311 = POcp35_310+RLcp35_311;
-    OMcp35_111 = OMcp35_110+qd[27]*ROcp35_126;
-    OMcp35_211 = OMcp35_210+qd[27]*ROcp35_226;
-    OMcp35_311 = OMcp35_310+qd[27]*ROcp35_326;
-    ORcp35_111 = OMcp35_210*RLcp35_311-OMcp35_310*RLcp35_211;
-    ORcp35_211 = -OMcp35_110*RLcp35_311+OMcp35_310*RLcp35_111;
-    ORcp35_311 = OMcp35_110*RLcp35_211-OMcp35_210*RLcp35_111;
-    VIcp35_111 = ORcp35_111+VIcp35_110;
-    VIcp35_211 = ORcp35_211+VIcp35_210;
-    VIcp35_311 = ORcp35_311+VIcp35_310;
-    RLcp35_112 = ROcp35_727*dpt[3][54];
-    RLcp35_212 = ROcp35_827*dpt[3][54];
-    RLcp35_312 = ROcp35_927*dpt[3][54];
-    POcp35_112 = POcp35_111+RLcp35_112;
-    POcp35_212 = POcp35_211+RLcp35_212;
-    POcp35_312 = POcp35_311+RLcp35_312;
-    OMcp35_112 = OMcp35_111+qd[28]*ROcp35_727;
-    OMcp35_212 = OMcp35_211+qd[28]*ROcp35_827;
-    OMcp35_312 = OMcp35_311+qd[28]*ROcp35_927;
-    ORcp35_112 = OMcp35_211*RLcp35_312-OMcp35_311*RLcp35_212;
-    ORcp35_212 = -OMcp35_111*RLcp35_312+OMcp35_311*RLcp35_112;
-    ORcp35_312 = OMcp35_111*RLcp35_212-OMcp35_211*RLcp35_112;
-    VIcp35_112 = ORcp35_112+VIcp35_111;
-    VIcp35_212 = ORcp35_212+VIcp35_211;
-    VIcp35_312 = ORcp35_312+VIcp35_311;
-    PxF11[1] = POcp35_112;
-    PxF11[2] = POcp35_212;
-    PxF11[3] = POcp35_312;
-    RxF11[1][1] = ROcp35_128;
-    RxF11[1][2] = ROcp35_228;
-    RxF11[1][3] = ROcp35_328;
-    RxF11[2][1] = ROcp35_428;
-    RxF11[2][2] = ROcp35_528;
-    RxF11[2][3] = ROcp35_628;
-    RxF11[3][1] = ROcp35_727;
-    RxF11[3][2] = ROcp35_827;
-    RxF11[3][3] = ROcp35_927;
-    VxF11[1] = VIcp35_112;
-    VxF11[2] = VIcp35_212;
-    VxF11[3] = VIcp35_312;
-    OMxF11[1] = OMcp35_112;
-    OMxF11[2] = OMcp35_212;
-    OMxF11[3] = OMcp35_312;
-    ROcp36_25 = S4*S5;
-    ROcp36_35 = -C4*S5;
-    ROcp36_85 = -S4*C5;
-    ROcp36_95 = C4*C5;
-    ROcp36_16 = C5*C6;
-    ROcp36_26 = ROcp36_25*C6+C4*S6;
-    ROcp36_36 = ROcp36_35*C6+S4*S6;
-    ROcp36_46 = -C5*S6;
-    ROcp36_56 = -ROcp36_25*S6+C4*C6;
-    ROcp36_66 = -ROcp36_35*S6+S4*C6;
-    ROcp36_419 = ROcp36_46*C19+S19*S5;
-    ROcp36_519 = ROcp36_56*C19+ROcp36_85*S19;
-    ROcp36_619 = ROcp36_66*C19+ROcp36_95*S19;
-    ROcp36_719 = -ROcp36_46*S19+C19*S5;
-    ROcp36_819 = -ROcp36_56*S19+ROcp36_85*C19;
-    ROcp36_919 = -ROcp36_66*S19+ROcp36_95*C19;
-    ROcp36_120 = ROcp36_16*C20-ROcp36_719*S20;
-    ROcp36_220 = ROcp36_26*C20-ROcp36_819*S20;
-    ROcp36_320 = ROcp36_36*C20-ROcp36_919*S20;
-    ROcp36_720 = ROcp36_16*S20+ROcp36_719*C20;
-    ROcp36_820 = ROcp36_26*S20+ROcp36_819*C20;
-    ROcp36_920 = ROcp36_36*S20+ROcp36_919*C20;
-    ROcp36_121 = ROcp36_120*C21+ROcp36_419*S21;
-    ROcp36_221 = ROcp36_220*C21+ROcp36_519*S21;
-    ROcp36_321 = ROcp36_320*C21+ROcp36_619*S21;
-    ROcp36_421 = -ROcp36_120*S21+ROcp36_419*C21;
-    ROcp36_521 = -ROcp36_220*S21+ROcp36_519*C21;
-    ROcp36_621 = -ROcp36_320*S21+ROcp36_619*C21;
-    ROcp36_126 = ROcp36_121*C26-ROcp36_720*S26;
-    ROcp36_226 = ROcp36_221*C26-ROcp36_820*S26;
-    ROcp36_326 = ROcp36_321*C26-ROcp36_920*S26;
-    ROcp36_726 = ROcp36_121*S26+ROcp36_720*C26;
-    ROcp36_826 = ROcp36_221*S26+ROcp36_820*C26;
-    ROcp36_926 = ROcp36_321*S26+ROcp36_920*C26;
-    ROcp36_427 = ROcp36_421*C27+ROcp36_726*S27;
-    ROcp36_527 = ROcp36_521*C27+ROcp36_826*S27;
-    ROcp36_627 = ROcp36_621*C27+ROcp36_926*S27;
-    ROcp36_727 = -ROcp36_421*S27+ROcp36_726*C27;
-    ROcp36_827 = -ROcp36_521*S27+ROcp36_826*C27;
-    ROcp36_927 = -ROcp36_621*S27+ROcp36_926*C27;
-    ROcp36_128 = ROcp36_126*C28+ROcp36_427*S28;
-    ROcp36_228 = ROcp36_226*C28+ROcp36_527*S28;
-    ROcp36_328 = ROcp36_326*C28+ROcp36_627*S28;
-    ROcp36_428 = -ROcp36_126*S28+ROcp36_427*C28;
-    ROcp36_528 = -ROcp36_226*S28+ROcp36_527*C28;
-    ROcp36_628 = -ROcp36_326*S28+ROcp36_627*C28;
-    ROcp36_129 = ROcp36_128*C29-ROcp36_727*S29;
-    ROcp36_229 = ROcp36_228*C29-ROcp36_827*S29;
-    ROcp36_329 = ROcp36_328*C29-ROcp36_927*S29;
-    ROcp36_729 = ROcp36_128*S29+ROcp36_727*C29;
-    ROcp36_829 = ROcp36_228*S29+ROcp36_827*C29;
-    ROcp36_929 = ROcp36_328*S29+ROcp36_927*C29;
-    OMcp36_25 = qd[5]*C4;
-    OMcp36_35 = qd[5]*S4;
-    OMcp36_16 = qd[4]+qd[6]*S5;
-    OMcp36_26 = OMcp36_25+qd[6]*ROcp36_85;
-    OMcp36_36 = OMcp36_35+qd[6]*ROcp36_95;
-    RLcp36_17 = ROcp36_16*dpt[1][4]+dpt[3][4]*S5;
-    RLcp36_27 = ROcp36_26*dpt[1][4]+ROcp36_85*dpt[3][4];
-    RLcp36_37 = ROcp36_36*dpt[1][4]+ROcp36_95*dpt[3][4];
-    POcp36_17 = q[1]+RLcp36_17;
-    POcp36_27 = q[2]+RLcp36_27;
-    POcp36_37 = q[3]+RLcp36_37;
-    OMcp36_17 = OMcp36_16+qd[19]*ROcp36_16;
-    OMcp36_27 = OMcp36_26+qd[19]*ROcp36_26;
-    OMcp36_37 = OMcp36_36+qd[19]*ROcp36_36;
-    ORcp36_17 = OMcp36_26*RLcp36_37-OMcp36_36*RLcp36_27;
-    ORcp36_27 = -OMcp36_16*RLcp36_37+OMcp36_36*RLcp36_17;
-    ORcp36_37 = OMcp36_16*RLcp36_27-OMcp36_26*RLcp36_17;
-    VIcp36_17 = qd[1]+ORcp36_17;
-    VIcp36_27 = qd[2]+ORcp36_27;
-    VIcp36_37 = qd[3]+ORcp36_37;
-    OMcp36_18 = OMcp36_17+qd[20]*ROcp36_419;
-    OMcp36_28 = OMcp36_27+qd[20]*ROcp36_519;
-    OMcp36_38 = OMcp36_37+qd[20]*ROcp36_619;
-    RLcp36_19 = ROcp36_720*dpt[3][36];
-    RLcp36_29 = ROcp36_820*dpt[3][36];
-    RLcp36_39 = ROcp36_920*dpt[3][36];
-    POcp36_19 = POcp36_17+RLcp36_19;
-    POcp36_29 = POcp36_27+RLcp36_29;
-    POcp36_39 = POcp36_37+RLcp36_39;
-    OMcp36_19 = OMcp36_18+qd[21]*ROcp36_720;
-    OMcp36_29 = OMcp36_28+qd[21]*ROcp36_820;
-    OMcp36_39 = OMcp36_38+qd[21]*ROcp36_920;
-    ORcp36_19 = OMcp36_28*RLcp36_39-OMcp36_38*RLcp36_29;
-    ORcp36_29 = -OMcp36_18*RLcp36_39+OMcp36_38*RLcp36_19;
-    ORcp36_39 = OMcp36_18*RLcp36_29-OMcp36_28*RLcp36_19;
-    VIcp36_19 = ORcp36_19+VIcp36_17;
-    VIcp36_29 = ORcp36_29+VIcp36_27;
-    VIcp36_39 = ORcp36_39+VIcp36_37;
-    RLcp36_110 = ROcp36_121*dpt[1][41]+ROcp36_421*dpt[2][41]+ROcp36_720*dpt[3][41];
-    RLcp36_210 = ROcp36_221*dpt[1][41]+ROcp36_521*dpt[2][41]+ROcp36_820*dpt[3][41];
-    RLcp36_310 = ROcp36_321*dpt[1][41]+ROcp36_621*dpt[2][41]+ROcp36_920*dpt[3][41];
-    POcp36_110 = POcp36_19+RLcp36_110;
-    POcp36_210 = POcp36_29+RLcp36_210;
-    POcp36_310 = POcp36_39+RLcp36_310;
-    OMcp36_110 = OMcp36_19+qd[26]*ROcp36_421;
-    OMcp36_210 = OMcp36_29+qd[26]*ROcp36_521;
-    OMcp36_310 = OMcp36_39+qd[26]*ROcp36_621;
-    ORcp36_110 = OMcp36_29*RLcp36_310-OMcp36_39*RLcp36_210;
-    ORcp36_210 = -OMcp36_19*RLcp36_310+OMcp36_39*RLcp36_110;
-    ORcp36_310 = OMcp36_19*RLcp36_210-OMcp36_29*RLcp36_110;
-    VIcp36_110 = ORcp36_110+VIcp36_19;
-    VIcp36_210 = ORcp36_210+VIcp36_29;
-    VIcp36_310 = ORcp36_310+VIcp36_39;
-    RLcp36_111 = ROcp36_421*dpt[2][52];
-    RLcp36_211 = ROcp36_521*dpt[2][52];
-    RLcp36_311 = ROcp36_621*dpt[2][52];
-    POcp36_111 = POcp36_110+RLcp36_111;
-    POcp36_211 = POcp36_210+RLcp36_211;
-    POcp36_311 = POcp36_310+RLcp36_311;
-    OMcp36_111 = OMcp36_110+qd[27]*ROcp36_126;
-    OMcp36_211 = OMcp36_210+qd[27]*ROcp36_226;
-    OMcp36_311 = OMcp36_310+qd[27]*ROcp36_326;
-    ORcp36_111 = OMcp36_210*RLcp36_311-OMcp36_310*RLcp36_211;
-    ORcp36_211 = -OMcp36_110*RLcp36_311+OMcp36_310*RLcp36_111;
-    ORcp36_311 = OMcp36_110*RLcp36_211-OMcp36_210*RLcp36_111;
-    VIcp36_111 = ORcp36_111+VIcp36_110;
-    VIcp36_211 = ORcp36_211+VIcp36_210;
-    VIcp36_311 = ORcp36_311+VIcp36_310;
-    RLcp36_112 = ROcp36_727*dpt[3][54];
-    RLcp36_212 = ROcp36_827*dpt[3][54];
-    RLcp36_312 = ROcp36_927*dpt[3][54];
-    POcp36_112 = POcp36_111+RLcp36_112;
-    POcp36_212 = POcp36_211+RLcp36_212;
-    POcp36_312 = POcp36_311+RLcp36_312;
-    OMcp36_112 = OMcp36_111+qd[28]*ROcp36_727;
-    OMcp36_212 = OMcp36_211+qd[28]*ROcp36_827;
-    OMcp36_312 = OMcp36_311+qd[28]*ROcp36_927;
-    ORcp36_112 = OMcp36_211*RLcp36_312-OMcp36_311*RLcp36_212;
-    ORcp36_212 = -OMcp36_111*RLcp36_312+OMcp36_311*RLcp36_112;
-    ORcp36_312 = OMcp36_111*RLcp36_212-OMcp36_211*RLcp36_112;
-    VIcp36_112 = ORcp36_112+VIcp36_111;
-    VIcp36_212 = ORcp36_212+VIcp36_211;
-    VIcp36_312 = ORcp36_312+VIcp36_311;
-    RLcp36_113 = ROcp36_727*dpt[3][57];
-    RLcp36_213 = ROcp36_827*dpt[3][57];
-    RLcp36_313 = ROcp36_927*dpt[3][57];
-    POcp36_113 = POcp36_112+RLcp36_113;
-    POcp36_213 = POcp36_212+RLcp36_213;
-    POcp36_313 = POcp36_312+RLcp36_313;
-    OMcp36_113 = OMcp36_112+qd[29]*ROcp36_428;
-    OMcp36_213 = OMcp36_212+qd[29]*ROcp36_528;
-    OMcp36_313 = OMcp36_312+qd[29]*ROcp36_628;
-    ORcp36_113 = OMcp36_212*RLcp36_313-OMcp36_312*RLcp36_213;
-    ORcp36_213 = -OMcp36_112*RLcp36_313+OMcp36_312*RLcp36_113;
-    ORcp36_313 = OMcp36_112*RLcp36_213-OMcp36_212*RLcp36_113;
-    VIcp36_113 = ORcp36_113+VIcp36_112;
-    VIcp36_213 = ORcp36_213+VIcp36_212;
-    VIcp36_313 = ORcp36_313+VIcp36_312;
-    PxF12[1] = POcp36_113;
-    PxF12[2] = POcp36_213;
-    PxF12[3] = POcp36_313;
-    RxF12[1][1] = ROcp36_129;
-    RxF12[1][2] = ROcp36_229;
-    RxF12[1][3] = ROcp36_329;
-    RxF12[2][1] = ROcp36_428;
-    RxF12[2][2] = ROcp36_528;
-    RxF12[2][3] = ROcp36_628;
-    RxF12[3][1] = ROcp36_729;
-    RxF12[3][2] = ROcp36_829;
-    RxF12[3][3] = ROcp36_929;
-    VxF12[1] = VIcp36_113;
-    VxF12[2] = VIcp36_213;
-    VxF12[3] = VIcp36_313;
-    OMxF12[1] = OMcp36_113;
-    OMxF12[2] = OMcp36_213;
-    OMxF12[3] = OMcp36_313;
- 
-    // output copy
-    for(i=1; i<=3; i++)
-    {
-        PxF_tab[0][i] = PxF1[i];
-        PxF_tab[1][i] = PxF2[i];
-        PxF_tab[2][i] = PxF3[i];
-        PxF_tab[3][i] = PxF4[i];
-        PxF_tab[4][i] = PxF5[i];
-        PxF_tab[5][i] = PxF6[i];
-        PxF_tab[6][i] = PxF7[i];
-        PxF_tab[7][i] = PxF8[i];
-        PxF_tab[8][i] = PxF9[i];
-        PxF_tab[9][i] = PxF10[i];
-        PxF_tab[10][i] = PxF11[i];
-        PxF_tab[11][i] = PxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        for(j=1; j<=3; j++)
-        {
-        RxF_tab[0][i][j] = RxF1[i][j];
-        RxF_tab[1][i][j] = RxF2[i][j];
-        RxF_tab[2][i][j] = RxF3[i][j];
-        RxF_tab[3][i][j] = RxF4[i][j];
-        RxF_tab[4][i][j] = RxF5[i][j];
-        RxF_tab[5][i][j] = RxF6[i][j];
-        RxF_tab[6][i][j] = RxF7[i][j];
-        RxF_tab[7][i][j] = RxF8[i][j];
-        RxF_tab[8][i][j] = RxF9[i][j];
-        RxF_tab[9][i][j] = RxF10[i][j];
-        RxF_tab[10][i][j] = RxF11[i][j];
-        RxF_tab[11][i][j] = RxF12[i][j];
-        }
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        VxF_tab[0][i] = VxF1[i];
-        VxF_tab[1][i] = VxF2[i];
-        VxF_tab[2][i] = VxF3[i];
-        VxF_tab[3][i] = VxF4[i];
-        VxF_tab[4][i] = VxF5[i];
-        VxF_tab[5][i] = VxF6[i];
-        VxF_tab[6][i] = VxF7[i];
-        VxF_tab[7][i] = VxF8[i];
-        VxF_tab[8][i] = VxF9[i];
-        VxF_tab[9][i] = VxF10[i];
-        VxF_tab[10][i] = VxF11[i];
-        VxF_tab[11][i] = VxF12[i];
-    }
-
-    for(i=1; i<=3; i++)
-    {
-        OMxF_tab[0][i] = OMxF1[i];
-        OMxF_tab[1][i] = OMxF2[i];
-        OMxF_tab[2][i] = OMxF3[i];
-        OMxF_tab[3][i] = OMxF4[i];
-        OMxF_tab[4][i] = OMxF5[i];
-        OMxF_tab[5][i] = OMxF6[i];
-        OMxF_tab[6][i] = OMxF7[i];
-        OMxF_tab[7][i] = OMxF8[i];
-        OMxF_tab[8][i] = OMxF9[i];
-        OMxF_tab[9][i] = OMxF10[i];
-        OMxF_tab[10][i] = OMxF11[i];
-        OMxF_tab[11][i] = OMxF12[i];
-    }
-}
diff --git a/symbolicR/short_feet_simp/CMakeLists.txt b/symbolicR/short_feet_simp/CMakeLists.txt
new file mode 100644
index 0000000..c963c74
--- /dev/null
+++ b/symbolicR/short_feet_simp/CMakeLists.txt
@@ -0,0 +1,97 @@
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# CMake for compiling the symbolic files of a robotran project in C
+# 
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                       LIBRARY MAIN CONFIGURATIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
+
+project(Project_symbolic)
+
+# flags
+if (UNIX)
+    set(CMAKE_C_FLAGS "-fPIC")
+    set(CMAKE_EXE_LINKER_FLAGS "-fPIC")
+endif (UNIX)
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                        SEPARATE COMPILATION
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+if ( FLAG_SEPARATE_SYMBOLIC )
+	set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
+	set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
+
+	add_subdirectory ( ${CMAKE_AUX}/flags/   ${CMAKE_AUX_BIN}/flags/   )
+	add_subdirectory ( ${CMAKE_AUX}/listing/ ${CMAKE_AUX_BIN}/listing/ )
+endif ( )
+
+# release of debug
+release_debug()
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         SET LIBRARY
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# list source files to compile
+init_src()
+increment_src( ${PROJECT_SOURCE_DIR} )
+increment_void_symbolic( ${PROJECT_SOURCE_DIR} )
+
+# list include directories (to find headers)
+init_include()
+increment_include( ${ROBOTRAN_SOURCE_DIR}/mbs_common )
+
+# include these directories
+include_directories ( ${INCLUDE_DIR} )
+include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
+
+if( FLAG_SHARED_LIB OR FLAG_SEPARATE_SYMBOLIC )
+
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+    add_library(Project_symbolic SHARED ${SOURCE_FILES})
+	
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_symbolic PROPERTIES SUFFIX .so)    
+    endif()    
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_SYMBOLIC))
+        add_custom_command(TARGET Project_symbolic POST_BUILD
+			COMMAND ${CMAKE_COMMAND} -E copy
+                        ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_symbolic.dll
+                        ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_symbolic.dll
+        )
+    endif()
+
+else()
+    add_library(Project_symbolic STATIC ${SOURCE_FILES})
+
+    # some symbolic routines call user functions (ext_forces, accelred, ...)
+    target_link_libraries(Project_symbolic Project_userfct)
+endif()
+
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         LINK LIBRARIES
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# MBSysC libraries
+if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
+    find_package( LibRobotranC REQUIRED )
+    add_definitions(${LIB_MBSYSC_DEFINITIONS})
+    target_link_libraries(Project_symbolic ${LIB_MBSYSC_MODULES})
+endif()
diff --git a/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.c b/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.c
new file mode 100644
index 0000000..fcc0b12
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.c
@@ -0,0 +1,1238 @@
+//
+//	MBsysTran - Release 8.1 (built: August 08, 2015)
+//
+//	Copyright 
+//	Universite catholique de Louvain (UCL) 
+//	Center for Research in Energy and Mechatronics (CEREM) 
+//	2, Place du Levant
+//	1348 Louvain-la-Neuve 
+//	Belgium 
+//
+//	http://www.robotran.be 
+//
+//	==> Generation Date: Fri Jan 27 19:16:35 2017
+//
+//	==> Project name: coman_short_feet_simp
+//
+//	==> Number of joints: 12
+//
+//	==> Function: F10 - Direct Dynamics (Explicit formulation) - RNEA + CP
+//
+
+#include <math.h> 
+
+#include "mbs_data.h"
+#include "mbs_project_interface.h"
+
+int mbs_accelred(MbsData *s, double tsim)
+{
+#include "mbs_accelred_coman_short_feet_simp.h"
+
+double  PxF1[4]; 
+double  RxF1[4][4]; 
+double  VxF1[4]; 
+double  OMxF1[4]; 
+double  AxF1[4]; 
+double  OMPxF1[4]; 
+double *SWr1; 
+double  PxF2[4]; 
+double  RxF2[4][4]; 
+double  VxF2[4]; 
+double  OMxF2[4]; 
+double  AxF2[4]; 
+double  OMPxF2[4]; 
+double *SWr2; 
+
+double *q, *qd, *qdd, *Qq;
+
+double **dpt, **l;
+double  *m;
+double **In;
+double  *g;
+
+int iter = 0; 
+
+q = s->q;
+qd = s->qd;
+qdd = s->qdd;
+Qq = s->Qq;
+
+dpt = s->dpt;
+l  = s->l;
+
+m = s->m;
+In = s->In;
+
+g = s->g;
+
+// Driven Variables 
+
+user_DrivenJoints(s,tsim);
+ 
+// Trigonometric functions
+
+S3 = sin(q[3]);
+C3 = cos(q[3]);
+S4 = sin(q[4]);
+C4 = cos(q[4]);
+S5 = sin(q[5]);
+C5 = cos(q[5]);
+S6 = sin(q[6]);
+C6 = cos(q[6]);
+S7 = sin(q[7]);
+C7 = cos(q[7]);
+S8 = sin(q[8]);
+C8 = cos(q[8]);
+S9 = sin(q[9]);
+C9 = cos(q[9]);
+S10 = sin(q[10]);
+C10 = cos(q[10]);
+S11 = sin(q[11]);
+C11 = cos(q[11]);
+S12 = sin(q[12]);
+C12 = cos(q[12]);
+ 
+// Augmented Joint Position Vectors
+
+ 
+// Forces and Torques computation 
+
+// Joint Forces 
+
+  Qq = user_JointForces(s,tsim);
+ 
+// Sensor Kinematics
+
+    ROcp1_14 = C3*C4-S3*S4;
+    ROcp1_34 = -C3*S4-S3*C4;
+    ROcp1_74 = C3*S4+S3*C4;
+    ROcp1_94 = C3*C4-S3*S4;
+    ROcp1_15 = ROcp1_14*C5-ROcp1_74*S5;
+    ROcp1_35 = ROcp1_34*C5-ROcp1_94*S5;
+    ROcp1_75 = ROcp1_14*S5+ROcp1_74*C5;
+    ROcp1_95 = ROcp1_34*S5+ROcp1_94*C5;
+    ROcp1_16 = ROcp1_15*C6-ROcp1_75*S6;
+    ROcp1_36 = ROcp1_35*C6-ROcp1_95*S6;
+    ROcp1_76 = ROcp1_15*S6+ROcp1_75*C6;
+    ROcp1_96 = ROcp1_35*S6+ROcp1_95*C6;
+    OMcp1_24 = qd[3]+qd[4];
+    RLcp1_15 = ROcp1_74*dpt[3][4];
+    RLcp1_35 = ROcp1_94*dpt[3][4];
+    POcp1_15 = q[1]+RLcp1_15;
+    POcp1_25 = dpt[2][1]+dpt[2][4];
+    POcp1_35 = q[2]+RLcp1_35;
+    OMcp1_25 = qd[5]+OMcp1_24;
+    ORcp1_15 = OMcp1_24*RLcp1_35;
+    ORcp1_35 = -OMcp1_24*RLcp1_15;
+    VIcp1_15 = qd[1]+ORcp1_15;
+    VIcp1_35 = qd[2]+ORcp1_35;
+    RLcp1_16 = ROcp1_75*dpt[3][5];
+    RLcp1_36 = ROcp1_95*dpt[3][5];
+    POcp1_16 = POcp1_15+RLcp1_16;
+    POcp1_36 = POcp1_35+RLcp1_36;
+    OMcp1_26 = qd[6]+OMcp1_25;
+    ORcp1_16 = OMcp1_25*RLcp1_36;
+    ORcp1_36 = -OMcp1_25*RLcp1_16;
+    VIcp1_16 = ORcp1_16+VIcp1_15;
+    VIcp1_36 = ORcp1_36+VIcp1_35;
+    RLcp1_17 = ROcp1_76*dpt[3][6];
+    RLcp1_37 = ROcp1_96*dpt[3][6];
+    POcp1_17 = POcp1_16+RLcp1_17;
+    POcp1_37 = POcp1_36+RLcp1_37;
+    ORcp1_17 = OMcp1_26*RLcp1_37;
+    ORcp1_37 = -OMcp1_26*RLcp1_17;
+    VIcp1_17 = ORcp1_17+VIcp1_16;
+    VIcp1_37 = ORcp1_37+VIcp1_36;
+    PxF1[1] = POcp1_17;
+    PxF1[2] = POcp1_25;
+    PxF1[3] = POcp1_37;
+    RxF1[1][1] = ROcp1_16;
+    RxF1[1][3] = ROcp1_36;
+    RxF1[2][2] = (1.0);
+    RxF1[3][1] = ROcp1_76;
+    RxF1[3][3] = ROcp1_96;
+    VxF1[1] = VIcp1_17;
+    VxF1[2] = 0;
+    VxF1[3] = VIcp1_37;
+    OMxF1[1] = 0;
+    OMxF1[2] = OMcp1_26;
+    OMxF1[3] = 0;
+    ROcp2_17 = C3*C7-S3*S7;
+    ROcp2_37 = -C3*S7-S3*C7;
+    ROcp2_77 = C3*S7+S3*C7;
+    ROcp2_97 = C3*C7-S3*S7;
+    ROcp2_18 = ROcp2_17*C8-ROcp2_77*S8;
+    ROcp2_38 = ROcp2_37*C8-ROcp2_97*S8;
+    ROcp2_78 = ROcp2_17*S8+ROcp2_77*C8;
+    ROcp2_98 = ROcp2_37*S8+ROcp2_97*C8;
+    ROcp2_19 = ROcp2_18*C9-ROcp2_78*S9;
+    ROcp2_39 = ROcp2_38*C9-ROcp2_98*S9;
+    ROcp2_79 = ROcp2_18*S9+ROcp2_78*C9;
+    ROcp2_99 = ROcp2_38*S9+ROcp2_98*C9;
+    OMcp2_24 = qd[3]+qd[7];
+    RLcp2_15 = ROcp2_77*dpt[3][7];
+    RLcp2_35 = ROcp2_97*dpt[3][7];
+    POcp2_15 = q[1]+RLcp2_15;
+    POcp2_25 = dpt[2][2]+dpt[2][7];
+    POcp2_35 = q[2]+RLcp2_35;
+    OMcp2_25 = qd[8]+OMcp2_24;
+    ORcp2_15 = OMcp2_24*RLcp2_35;
+    ORcp2_35 = -OMcp2_24*RLcp2_15;
+    VIcp2_15 = qd[1]+ORcp2_15;
+    VIcp2_35 = qd[2]+ORcp2_35;
+    RLcp2_16 = ROcp2_78*dpt[3][8];
+    RLcp2_36 = ROcp2_98*dpt[3][8];
+    POcp2_16 = POcp2_15+RLcp2_16;
+    POcp2_36 = POcp2_35+RLcp2_36;
+    OMcp2_26 = qd[9]+OMcp2_25;
+    ORcp2_16 = OMcp2_25*RLcp2_36;
+    ORcp2_36 = -OMcp2_25*RLcp2_16;
+    VIcp2_16 = ORcp2_16+VIcp2_15;
+    VIcp2_36 = ORcp2_36+VIcp2_35;
+    RLcp2_17 = ROcp2_79*dpt[3][9];
+    RLcp2_37 = ROcp2_99*dpt[3][9];
+    POcp2_17 = POcp2_16+RLcp2_17;
+    POcp2_37 = POcp2_36+RLcp2_37;
+    ORcp2_17 = OMcp2_26*RLcp2_37;
+    ORcp2_37 = -OMcp2_26*RLcp2_17;
+    VIcp2_17 = ORcp2_17+VIcp2_16;
+    VIcp2_37 = ORcp2_37+VIcp2_36;
+    PxF2[1] = POcp2_17;
+    PxF2[2] = POcp2_25;
+    PxF2[3] = POcp2_37;
+    RxF2[1][1] = ROcp2_19;
+    RxF2[1][3] = ROcp2_39;
+    RxF2[2][2] = (1.0);
+    RxF2[3][1] = ROcp2_79;
+    RxF2[3][3] = ROcp2_99;
+    VxF2[1] = VIcp2_17;
+    VxF2[2] = 0;
+    VxF2[3] = VIcp2_37;
+    OMxF2[1] = 0;
+    OMxF2[2] = OMcp2_26;
+    OMxF2[3] = 0;
+ 
+// Sensor Forces 
+
+  SWr1 = user_ExtForces(PxF1,RxF1,VxF1,OMxF1,AxF1,OMPxF1,s,tsim,1);
+  SWr2 = user_ExtForces(PxF2,RxF2,VxF2,OMxF2,AxF2,OMPxF2,s,tsim,2);
+  xfrc11 = RxF1[1][1]*SWr1[1]+RxF1[1][3]*SWr1[3];
+  xfrc21 = RxF1[2][2]*SWr1[2];
+  xfrc31 = RxF1[3][1]*SWr1[1]+RxF1[3][3]*SWr1[3];
+  frcext_1_6 = xfrc11;
+  frcext_2_6 = xfrc21;
+  frcext_3_6 = xfrc31;
+  xtrq11 = RxF1[1][1]*SWr1[4]+RxF1[1][3]*SWr1[6];
+  xtrq21 = RxF1[2][2]*SWr1[5];
+  xtrq31 = RxF1[3][1]*SWr1[4]+RxF1[3][3]*SWr1[6];
+  trqext_1_6 = xtrq11-xfrc21*(SWr1[9]-l[3][6])+xfrc31*SWr1[8];
+  trqext_2_6 = xtrq21+xfrc11*(SWr1[9]-l[3][6])-xfrc31*(SWr1[7]-l[1][6]);
+  trqext_3_6 = xtrq31-xfrc11*SWr1[8]+xfrc21*(SWr1[7]-l[1][6]);
+  xfrc12 = RxF2[1][1]*SWr2[1]+RxF2[1][3]*SWr2[3];
+  xfrc22 = RxF2[2][2]*SWr2[2];
+  xfrc32 = RxF2[3][1]*SWr2[1]+RxF2[3][3]*SWr2[3];
+  frcext_1_9 = xfrc12;
+  frcext_2_9 = xfrc22;
+  frcext_3_9 = xfrc32;
+  xtrq12 = RxF2[1][1]*SWr2[4]+RxF2[1][3]*SWr2[6];
+  xtrq22 = RxF2[2][2]*SWr2[5];
+  xtrq32 = RxF2[3][1]*SWr2[4]+RxF2[3][3]*SWr2[6];
+  trqext_1_9 = xtrq12-xfrc22*(SWr2[9]-l[3][9])+xfrc32*SWr2[8];
+  trqext_2_9 = xtrq22+xfrc12*(SWr2[9]-l[3][9])-xfrc32*(SWr2[7]-l[1][9]);
+  trqext_3_9 = xtrq32-xfrc12*SWr2[8]+xfrc22*(SWr2[7]-l[1][9]);
+ 
+// Forward Kinematics
+
+  BS13 = -qd[3]*qd[3];
+  BS93 = -qd[3]*qd[3];
+  AlF13 = g[3]*S3;
+  AlF33 = -g[3]*C3;
+  OM24 = qd[3]+qd[4];
+  BS14 = -OM24*OM24;
+  BS94 = -OM24*OM24;
+  AlF14 = AlF13*C4-AlF33*S4;
+  AlF34 = AlF13*S4+AlF33*C4;
+  AlM14_1 = C3*C4-S3*S4;
+  AlM34_1 = C3*S4+S3*C4;
+  AlM14_2 = -C3*S4-S3*C4;
+  AlM34_2 = C3*C4-S3*S4;
+  OM25 = qd[5]+OM24;
+  BS15 = -OM25*OM25;
+  BS95 = -OM25*OM25;
+  AlF15 = AlF14*C5-S5*(AlF34+BS94*dpt[3][4]);
+  AlF35 = AlF14*S5+C5*(AlF34+BS94*dpt[3][4]);
+  AlM15_1 = AlM14_1*C5-AlM34_1*S5;
+  AlM35_1 = AlM14_1*S5+AlM34_1*C5;
+  AlM15_2 = AlM14_2*C5-AlM34_2*S5;
+  AlM35_2 = AlM14_2*S5+AlM34_2*C5;
+  AlM15_3 = dpt[3][4]*C5;
+  AlM35_3 = dpt[3][4]*S5;
+  AlM15_4 = dpt[3][4]*C5;
+  AlM35_4 = dpt[3][4]*S5;
+  OM26 = qd[6]+OM25;
+  BS16 = -OM26*OM26;
+  BS96 = -OM26*OM26;
+  AlF16 = AlF15*C6-S6*(AlF35+BS95*dpt[3][5]);
+  AlF36 = AlF15*S6+C6*(AlF35+BS95*dpt[3][5]);
+  AlM16_1 = AlM15_1*C6-AlM35_1*S6;
+  AlM36_1 = AlM15_1*S6+AlM35_1*C6;
+  AlM16_2 = AlM15_2*C6-AlM35_2*S6;
+  AlM36_2 = AlM15_2*S6+AlM35_2*C6;
+  AlM16_3 = -AlM35_3*S6+C6*(AlM15_3+dpt[3][5]);
+  AlM36_3 = AlM35_3*C6+S6*(AlM15_3+dpt[3][5]);
+  AlM16_4 = -AlM35_4*S6+C6*(AlM15_4+dpt[3][5]);
+  AlM36_4 = AlM35_4*C6+S6*(AlM15_4+dpt[3][5]);
+  AlM16_5 = dpt[3][5]*C6;
+  AlM36_5 = dpt[3][5]*S6;
+  OM27 = qd[3]+qd[7];
+  BS17 = -OM27*OM27;
+  BS97 = -OM27*OM27;
+  AlF17 = AlF13*C7-AlF33*S7;
+  AlF37 = AlF13*S7+AlF33*C7;
+  AlM17_1 = C3*C7-S3*S7;
+  AlM37_1 = C3*S7+S3*C7;
+  AlM17_2 = -C3*S7-S3*C7;
+  AlM37_2 = C3*C7-S3*S7;
+  OM28 = qd[8]+OM27;
+  BS18 = -OM28*OM28;
+  BS98 = -OM28*OM28;
+  AlF18 = AlF17*C8-S8*(AlF37+BS97*dpt[3][7]);
+  AlF38 = AlF17*S8+C8*(AlF37+BS97*dpt[3][7]);
+  AlM18_1 = AlM17_1*C8-AlM37_1*S8;
+  AlM38_1 = AlM17_1*S8+AlM37_1*C8;
+  AlM18_2 = AlM17_2*C8-AlM37_2*S8;
+  AlM38_2 = AlM17_2*S8+AlM37_2*C8;
+  AlM18_3 = dpt[3][7]*C8;
+  AlM38_3 = dpt[3][7]*S8;
+  AlM18_7 = dpt[3][7]*C8;
+  AlM38_7 = dpt[3][7]*S8;
+  OM29 = qd[9]+OM28;
+  BS19 = -OM29*OM29;
+  BS99 = -OM29*OM29;
+  AlF19 = AlF18*C9-S9*(AlF38+BS98*dpt[3][8]);
+  AlF39 = AlF18*S9+C9*(AlF38+BS98*dpt[3][8]);
+  AlM19_1 = AlM18_1*C9-AlM38_1*S9;
+  AlM39_1 = AlM18_1*S9+AlM38_1*C9;
+  AlM19_2 = AlM18_2*C9-AlM38_2*S9;
+  AlM39_2 = AlM18_2*S9+AlM38_2*C9;
+  AlM19_3 = -AlM38_3*S9+C9*(AlM18_3+dpt[3][8]);
+  AlM39_3 = AlM38_3*C9+S9*(AlM18_3+dpt[3][8]);
+  AlM19_7 = -AlM38_7*S9+C9*(AlM18_7+dpt[3][8]);
+  AlM39_7 = AlM38_7*C9+S9*(AlM18_7+dpt[3][8]);
+  AlM19_8 = dpt[3][8]*C9;
+  AlM39_8 = dpt[3][8]*S9;
+  OM210 = qd[10]+qd[3];
+  BS110 = -OM210*OM210;
+  BS910 = -OM210*OM210;
+  AlF110 = C10*(AlF13+BS13*dpt[1][3])-S10*(AlF33+BS93*dpt[3][3]);
+  AlF310 = C10*(AlF33+BS93*dpt[3][3])+S10*(AlF13+BS13*dpt[1][3]);
+  AlM110_1 = C10*C3-S10*S3;
+  AlM310_1 = C10*S3+S10*C3;
+  AlM110_2 = -C10*S3-S10*C3;
+  AlM310_2 = C10*C3-S10*S3;
+  AlM110_3 = dpt[1][3]*S10+dpt[3][3]*C10;
+  AlM310_3 = -dpt[1][3]*C10+dpt[3][3]*S10;
+  OM211 = qd[11]+OM210;
+  BS111 = -OM211*OM211;
+  BS911 = -OM211*OM211;
+  AlF111 = C11*(AlF110+BS110*dpt[1][10])-S11*(AlF310+BS910*dpt[3][10]);
+  AlF311 = C11*(AlF310+BS910*dpt[3][10])+S11*(AlF110+BS110*dpt[1][10]);
+  AlM111_1 = AlM110_1*C11-AlM310_1*S11;
+  AlM311_1 = AlM110_1*S11+AlM310_1*C11;
+  AlM111_2 = AlM110_2*C11-AlM310_2*S11;
+  AlM311_2 = AlM110_2*S11+AlM310_2*C11;
+  AlM111_3 = C11*(AlM110_3+dpt[3][10])-S11*(AlM310_3-dpt[1][10]);
+  AlM311_3 = C11*(AlM310_3-dpt[1][10])+S11*(AlM110_3+dpt[3][10]);
+  AlM111_10 = dpt[1][10]*S11+dpt[3][10]*C11;
+  AlM311_10 = -dpt[1][10]*C11+dpt[3][10]*S11;
+  OM212 = qd[12]+OM210;
+  BS112 = -OM212*OM212;
+  BS912 = -OM212*OM212;
+  AlF112 = C12*(AlF110+BS110*dpt[1][11])-S12*(AlF310+BS910*dpt[3][11]);
+  AlF312 = C12*(AlF310+BS910*dpt[3][11])+S12*(AlF110+BS110*dpt[1][11]);
+  AlM112_1 = AlM110_1*C12-AlM310_1*S12;
+  AlM312_1 = AlM110_1*S12+AlM310_1*C12;
+  AlM112_2 = AlM110_2*C12-AlM310_2*S12;
+  AlM312_2 = AlM110_2*S12+AlM310_2*C12;
+  AlM112_3 = C12*(AlM110_3+dpt[3][11])-S12*(AlM310_3-dpt[1][11]);
+  AlM312_3 = C12*(AlM310_3-dpt[1][11])+S12*(AlM110_3+dpt[3][11]);
+  AlM112_10 = dpt[1][11]*S12+dpt[3][11]*C12;
+  AlM312_10 = -dpt[1][11]*C12+dpt[3][11]*S12;
+ 
+// Backward Dynamics
+
+  FA112 = m[12]*(AlF112+BS112*l[1][12]);
+  FA312 = m[12]*(AlF312+BS912*l[3][12]);
+  CF212 = FA112*l[3][12]-FA312*l[1][12];
+  FB112_1 = m[12]*AlM112_1;
+  FB312_1 = m[12]*AlM312_1;
+  CM212_1 = FB112_1*l[3][12]-FB312_1*l[1][12];
+  FB112_2 = m[12]*AlM112_2;
+  FB312_2 = m[12]*AlM312_2;
+  CM212_2 = FB112_2*l[3][12]-FB312_2*l[1][12];
+  FB112_3 = m[12]*(AlM112_3+l[3][12]);
+  FB312_3 = m[12]*(AlM312_3-l[1][12]);
+  CM212_3 = In[5][12]+FB112_3*l[3][12]-FB312_3*l[1][12];
+  FB112_10 = m[12]*(AlM112_10+l[3][12]);
+  FB312_10 = m[12]*(AlM312_10-l[1][12]);
+  CM212_10 = In[5][12]+FB112_10*l[3][12]-FB312_10*l[1][12];
+  FB112_12 = m[12]*l[3][12];
+  FB312_12 = -m[12]*l[1][12];
+  CM212_12 = In[5][12]+FB112_12*l[3][12]-FB312_12*l[1][12];
+  FA111 = m[11]*(AlF111+BS111*l[1][11]);
+  FA311 = m[11]*(AlF311+BS911*l[3][11]);
+  CF211 = FA111*l[3][11]-FA311*l[1][11];
+  FB111_1 = m[11]*AlM111_1;
+  FB311_1 = m[11]*AlM311_1;
+  CM211_1 = FB111_1*l[3][11]-FB311_1*l[1][11];
+  FB111_2 = m[11]*AlM111_2;
+  FB311_2 = m[11]*AlM311_2;
+  CM211_2 = FB111_2*l[3][11]-FB311_2*l[1][11];
+  FB111_3 = m[11]*(AlM111_3+l[3][11]);
+  FB311_3 = m[11]*(AlM311_3-l[1][11]);
+  CM211_3 = In[5][11]+FB111_3*l[3][11]-FB311_3*l[1][11];
+  FB111_10 = m[11]*(AlM111_10+l[3][11]);
+  FB311_10 = m[11]*(AlM311_10-l[1][11]);
+  CM211_10 = In[5][11]+FB111_10*l[3][11]-FB311_10*l[1][11];
+  FB111_11 = m[11]*l[3][11];
+  FB311_11 = -m[11]*l[1][11];
+  CM211_11 = In[5][11]+FB111_11*l[3][11]-FB311_11*l[1][11];
+  FA110 = m[10]*(AlF110+BS110*l[1][10]);
+  FA310 = m[10]*(AlF310+BS910*l[3][10]);
+  FF110 = FA110+FA111*C11+FA112*C12+FA311*S11+FA312*S12;
+  FF310 = FA310-FA111*S11-FA112*S12+FA311*C11+FA312*C12;
+  CF210 = CF211+CF212+FA110*l[3][10]-FA310*l[1][10]-dpt[1][10]*(-FA111*S11+FA311*C11)-dpt[1][11]*(-FA112*S12+FA312*C12)+
+ dpt[3][10]*(FA111*C11+FA311*S11)+dpt[3][11]*(FA112*C12+FA312*S12);
+  FB110_1 = m[10]*AlM110_1;
+  FB310_1 = m[10]*AlM310_1;
+  FM110_1 = FB110_1+FB111_1*C11+FB112_1*C12+FB311_1*S11+FB312_1*S12;
+  FM310_1 = FB310_1-FB111_1*S11-FB112_1*S12+FB311_1*C11+FB312_1*C12;
+  CM210_1 = CM211_1+CM212_1+FB110_1*l[3][10]-FB310_1*l[1][10]-dpt[1][10]*(-FB111_1*S11+FB311_1*C11)-dpt[1][11]*(-FB112_1
+ *S12+FB312_1*C12)+dpt[3][10]*(FB111_1*C11+FB311_1*S11)+dpt[3][11]*(FB112_1*C12+FB312_1*S12);
+  FB110_2 = m[10]*AlM110_2;
+  FB310_2 = m[10]*AlM310_2;
+  FM110_2 = FB110_2+FB111_2*C11+FB112_2*C12+FB311_2*S11+FB312_2*S12;
+  FM310_2 = FB310_2-FB111_2*S11-FB112_2*S12+FB311_2*C11+FB312_2*C12;
+  CM210_2 = CM211_2+CM212_2+FB110_2*l[3][10]-FB310_2*l[1][10]-dpt[1][10]*(-FB111_2*S11+FB311_2*C11)-dpt[1][11]*(-FB112_2
+ *S12+FB312_2*C12)+dpt[3][10]*(FB111_2*C11+FB311_2*S11)+dpt[3][11]*(FB112_2*C12+FB312_2*S12);
+  FB110_3 = m[10]*(AlM110_3+l[3][10]);
+  FB310_3 = m[10]*(AlM310_3-l[1][10]);
+  FM110_3 = FB110_3+FB111_3*C11+FB112_3*C12+FB311_3*S11+FB312_3*S12;
+  FM310_3 = FB310_3-FB111_3*S11-FB112_3*S12+FB311_3*C11+FB312_3*C12;
+  CM210_3 = In[5][10]+CM211_3+CM212_3+FB110_3*l[3][10]-FB310_3*l[1][10]-dpt[1][10]*(-FB111_3*S11+FB311_3*C11)-dpt[1][11]
+ *(-FB112_3*S12+FB312_3*C12)+dpt[3][10]*(FB111_3*C11+FB311_3*S11)+dpt[3][11]*(FB112_3*C12+FB312_3*S12);
+  FB110_10 = m[10]*l[3][10];
+  FB310_10 = -m[10]*l[1][10];
+  CM210_10 = In[5][10]+CM211_10+CM212_10+FB110_10*l[3][10]-FB310_10*l[1][10]-dpt[1][10]*(-FB111_10*S11+FB311_10*C11)-
+ dpt[1][11]*(-FB112_10*S12+FB312_10*C12)+dpt[3][10]*(FB111_10*C11+FB311_10*S11)+dpt[3][11]*(FB112_10*C12+FB312_10*S12);
+  FA19 = -frcext_1_9+m[9]*(AlF19+BS19*l[1][9]);
+  FA39 = -frcext_3_9+m[9]*(AlF39+BS99*l[3][9]);
+  CF29 = -trqext_2_9+FA19*l[3][9]-FA39*l[1][9];
+  FB19_1 = m[9]*AlM19_1;
+  FB39_1 = m[9]*AlM39_1;
+  CM29_1 = FB19_1*l[3][9]-FB39_1*l[1][9];
+  FB19_2 = m[9]*AlM19_2;
+  FB39_2 = m[9]*AlM39_2;
+  CM29_2 = FB19_2*l[3][9]-FB39_2*l[1][9];
+  FB19_3 = m[9]*(AlM19_3+l[3][9]);
+  FB39_3 = m[9]*(AlM39_3-l[1][9]);
+  CM29_3 = In[5][9]+FB19_3*l[3][9]-FB39_3*l[1][9];
+  FB19_7 = m[9]*(AlM19_7+l[3][9]);
+  FB39_7 = m[9]*(AlM39_7-l[1][9]);
+  CM29_7 = In[5][9]+FB19_7*l[3][9]-FB39_7*l[1][9];
+  FB19_8 = m[9]*(AlM19_8+l[3][9]);
+  FB39_8 = m[9]*(AlM39_8-l[1][9]);
+  CM29_8 = In[5][9]+FB19_8*l[3][9]-FB39_8*l[1][9];
+  FB19_9 = m[9]*l[3][9];
+  FB39_9 = -m[9]*l[1][9];
+  CM29_9 = In[5][9]+FB19_9*l[3][9]-FB39_9*l[1][9];
+  FA18 = m[8]*(AlF18+BS18*l[1][8]);
+  FA38 = m[8]*(AlF38+BS98*l[3][8]);
+  FF18 = FA18+FA19*C9+FA39*S9;
+  FF38 = FA38-FA19*S9+FA39*C9;
+  CF28 = CF29+FA18*l[3][8]-FA38*l[1][8]+dpt[3][8]*(FA19*C9+FA39*S9);
+  FB18_1 = m[8]*AlM18_1;
+  FB38_1 = m[8]*AlM38_1;
+  FM18_1 = FB18_1+FB19_1*C9+FB39_1*S9;
+  FM38_1 = FB38_1-FB19_1*S9+FB39_1*C9;
+  CM28_1 = CM29_1+FB18_1*l[3][8]-FB38_1*l[1][8]+dpt[3][8]*(FB19_1*C9+FB39_1*S9);
+  FB18_2 = m[8]*AlM18_2;
+  FB38_2 = m[8]*AlM38_2;
+  FM18_2 = FB18_2+FB19_2*C9+FB39_2*S9;
+  FM38_2 = FB38_2-FB19_2*S9+FB39_2*C9;
+  CM28_2 = CM29_2+FB18_2*l[3][8]-FB38_2*l[1][8]+dpt[3][8]*(FB19_2*C9+FB39_2*S9);
+  FB18_3 = m[8]*(AlM18_3+l[3][8]);
+  FB38_3 = m[8]*(AlM38_3-l[1][8]);
+  FM18_3 = FB18_3+FB19_3*C9+FB39_3*S9;
+  FM38_3 = FB38_3-FB19_3*S9+FB39_3*C9;
+  CM28_3 = In[5][8]+CM29_3+FB18_3*l[3][8]-FB38_3*l[1][8]+dpt[3][8]*(FB19_3*C9+FB39_3*S9);
+  FB18_7 = m[8]*(AlM18_7+l[3][8]);
+  FB38_7 = m[8]*(AlM38_7-l[1][8]);
+  FM18_7 = FB18_7+FB19_7*C9+FB39_7*S9;
+  FM38_7 = FB38_7-FB19_7*S9+FB39_7*C9;
+  CM28_7 = In[5][8]+CM29_7+FB18_7*l[3][8]-FB38_7*l[1][8]+dpt[3][8]*(FB19_7*C9+FB39_7*S9);
+  FB18_8 = m[8]*l[3][8];
+  FB38_8 = -m[8]*l[1][8];
+  CM28_8 = In[5][8]+CM29_8+FB18_8*l[3][8]-FB38_8*l[1][8]+dpt[3][8]*(FB19_8*C9+FB39_8*S9);
+  FA17 = m[7]*(AlF17+BS17*l[1][7]);
+  FA37 = m[7]*(AlF37+BS97*l[3][7]);
+  FF17 = FA17+FF18*C8+FF38*S8;
+  FF37 = FA37-FF18*S8+FF38*C8;
+  CF27 = CF28+FA17*l[3][7]-FA37*l[1][7]+dpt[3][7]*(FF18*C8+FF38*S8);
+  FB17_1 = m[7]*AlM17_1;
+  FB37_1 = m[7]*AlM37_1;
+  FM17_1 = FB17_1+FM18_1*C8+FM38_1*S8;
+  FM37_1 = FB37_1-FM18_1*S8+FM38_1*C8;
+  CM27_1 = CM28_1+FB17_1*l[3][7]-FB37_1*l[1][7]+dpt[3][7]*(FM18_1*C8+FM38_1*S8);
+  FB17_2 = m[7]*AlM17_2;
+  FB37_2 = m[7]*AlM37_2;
+  FM17_2 = FB17_2+FM18_2*C8+FM38_2*S8;
+  FM37_2 = FB37_2-FM18_2*S8+FM38_2*C8;
+  CM27_2 = CM28_2+FB17_2*l[3][7]-FB37_2*l[1][7]+dpt[3][7]*(FM18_2*C8+FM38_2*S8);
+  FB17_3 = m[7]*l[3][7];
+  FB37_3 = -m[7]*l[1][7];
+  CM27_3 = In[5][7]+CM28_3+FB17_3*l[3][7]-FB37_3*l[1][7]+dpt[3][7]*(FM18_3*C8+FM38_3*S8);
+  FB17_7 = m[7]*l[3][7];
+  FB37_7 = -m[7]*l[1][7];
+  CM27_7 = In[5][7]+CM28_7+FB17_7*l[3][7]-FB37_7*l[1][7]+dpt[3][7]*(FM18_7*C8+FM38_7*S8);
+  FA16 = -frcext_1_6+m[6]*(AlF16+BS16*l[1][6]);
+  FA36 = -frcext_3_6+m[6]*(AlF36+BS96*l[3][6]);
+  CF26 = -trqext_2_6+FA16*l[3][6]-FA36*l[1][6];
+  FB16_1 = m[6]*AlM16_1;
+  FB36_1 = m[6]*AlM36_1;
+  CM26_1 = FB16_1*l[3][6]-FB36_1*l[1][6];
+  FB16_2 = m[6]*AlM16_2;
+  FB36_2 = m[6]*AlM36_2;
+  CM26_2 = FB16_2*l[3][6]-FB36_2*l[1][6];
+  FB16_3 = m[6]*(AlM16_3+l[3][6]);
+  FB36_3 = m[6]*(AlM36_3-l[1][6]);
+  CM26_3 = In[5][6]+FB16_3*l[3][6]-FB36_3*l[1][6];
+  FB16_4 = m[6]*(AlM16_4+l[3][6]);
+  FB36_4 = m[6]*(AlM36_4-l[1][6]);
+  CM26_4 = In[5][6]+FB16_4*l[3][6]-FB36_4*l[1][6];
+  FB16_5 = m[6]*(AlM16_5+l[3][6]);
+  FB36_5 = m[6]*(AlM36_5-l[1][6]);
+  CM26_5 = In[5][6]+FB16_5*l[3][6]-FB36_5*l[1][6];
+  FB16_6 = m[6]*l[3][6];
+  FB36_6 = -m[6]*l[1][6];
+  CM26_6 = In[5][6]+FB16_6*l[3][6]-FB36_6*l[1][6];
+  FA15 = m[5]*(AlF15+BS15*l[1][5]);
+  FA35 = m[5]*(AlF35+BS95*l[3][5]);
+  FF15 = FA15+FA16*C6+FA36*S6;
+  FF35 = FA35-FA16*S6+FA36*C6;
+  CF25 = CF26+FA15*l[3][5]-FA35*l[1][5]+dpt[3][5]*(FA16*C6+FA36*S6);
+  FB15_1 = m[5]*AlM15_1;
+  FB35_1 = m[5]*AlM35_1;
+  FM15_1 = FB15_1+FB16_1*C6+FB36_1*S6;
+  FM35_1 = FB35_1-FB16_1*S6+FB36_1*C6;
+  CM25_1 = CM26_1+FB15_1*l[3][5]-FB35_1*l[1][5]+dpt[3][5]*(FB16_1*C6+FB36_1*S6);
+  FB15_2 = m[5]*AlM15_2;
+  FB35_2 = m[5]*AlM35_2;
+  FM15_2 = FB15_2+FB16_2*C6+FB36_2*S6;
+  FM35_2 = FB35_2-FB16_2*S6+FB36_2*C6;
+  CM25_2 = CM26_2+FB15_2*l[3][5]-FB35_2*l[1][5]+dpt[3][5]*(FB16_2*C6+FB36_2*S6);
+  FB15_3 = m[5]*(AlM15_3+l[3][5]);
+  FB35_3 = m[5]*(AlM35_3-l[1][5]);
+  FM15_3 = FB15_3+FB16_3*C6+FB36_3*S6;
+  FM35_3 = FB35_3-FB16_3*S6+FB36_3*C6;
+  CM25_3 = In[5][5]+CM26_3+FB15_3*l[3][5]-FB35_3*l[1][5]+dpt[3][5]*(FB16_3*C6+FB36_3*S6);
+  FB15_4 = m[5]*(AlM15_4+l[3][5]);
+  FB35_4 = m[5]*(AlM35_4-l[1][5]);
+  FM15_4 = FB15_4+FB16_4*C6+FB36_4*S6;
+  FM35_4 = FB35_4-FB16_4*S6+FB36_4*C6;
+  CM25_4 = In[5][5]+CM26_4+FB15_4*l[3][5]-FB35_4*l[1][5]+dpt[3][5]*(FB16_4*C6+FB36_4*S6);
+  FB15_5 = m[5]*l[3][5];
+  FB35_5 = -m[5]*l[1][5];
+  CM25_5 = In[5][5]+CM26_5+FB15_5*l[3][5]-FB35_5*l[1][5]+dpt[3][5]*(FB16_5*C6+FB36_5*S6);
+  FA14 = m[4]*(AlF14+BS14*l[1][4]);
+  FA34 = m[4]*(AlF34+BS94*l[3][4]);
+  FF14 = FA14+FF15*C5+FF35*S5;
+  FF34 = FA34-FF15*S5+FF35*C5;
+  CF24 = CF25+FA14*l[3][4]-FA34*l[1][4]+dpt[3][4]*(FF15*C5+FF35*S5);
+  FB14_1 = m[4]*AlM14_1;
+  FB34_1 = m[4]*AlM34_1;
+  FM14_1 = FB14_1+FM15_1*C5+FM35_1*S5;
+  FM34_1 = FB34_1-FM15_1*S5+FM35_1*C5;
+  CM24_1 = CM25_1+FB14_1*l[3][4]-FB34_1*l[1][4]+dpt[3][4]*(FM15_1*C5+FM35_1*S5);
+  FB14_2 = m[4]*AlM14_2;
+  FB34_2 = m[4]*AlM34_2;
+  FM14_2 = FB14_2+FM15_2*C5+FM35_2*S5;
+  FM34_2 = FB34_2-FM15_2*S5+FM35_2*C5;
+  CM24_2 = CM25_2+FB14_2*l[3][4]-FB34_2*l[1][4]+dpt[3][4]*(FM15_2*C5+FM35_2*S5);
+  FB14_3 = m[4]*l[3][4];
+  FB34_3 = -m[4]*l[1][4];
+  CM24_3 = In[5][4]+CM25_3+FB14_3*l[3][4]-FB34_3*l[1][4]+dpt[3][4]*(FM15_3*C5+FM35_3*S5);
+  FB14_4 = m[4]*l[3][4];
+  FB34_4 = -m[4]*l[1][4];
+  CM24_4 = In[5][4]+CM25_4+FB14_4*l[3][4]-FB34_4*l[1][4]+dpt[3][4]*(FM15_4*C5+FM35_4*S5);
+  FA13 = m[3]*(AlF13+BS13*l[1][3]);
+  FA33 = m[3]*(AlF33+BS93*l[3][3]);
+  FF13 = FA13+FF110*C10+FF14*C4+FF17*C7+FF310*S10+FF34*S4+FF37*S7;
+  FF33 = FA33-FF110*S10-FF14*S4-FF17*S7+FF310*C10+FF34*C4+FF37*C7;
+  CF23 = CF210+CF24+CF27+FA13*l[3][3]-FA33*l[1][3]-dpt[1][3]*(-FF110*S10+FF310*C10)+dpt[3][3]*(FF110*C10+FF310*S10);
+  FB13_1 = m[3]*C3;
+  FB33_1 = m[3]*S3;
+  FM13_1 = FB13_1+FM110_1*C10+FM14_1*C4+FM17_1*C7+FM310_1*S10+FM34_1*S4+FM37_1*S7;
+  FM33_1 = FB33_1-FM110_1*S10-FM14_1*S4-FM17_1*S7+FM310_1*C10+FM34_1*C4+FM37_1*C7;
+  CM23_1 = CM210_1+CM24_1+CM27_1+FB13_1*l[3][3]-FB33_1*l[1][3]-dpt[1][3]*(-FM110_1*S10+FM310_1*C10)+dpt[3][3]*(FM110_1*
+ C10+FM310_1*S10);
+  FB13_2 = -m[3]*S3;
+  FB33_2 = m[3]*C3;
+  FM13_2 = FB13_2+FM110_2*C10+FM14_2*C4+FM17_2*C7+FM310_2*S10+FM34_2*S4+FM37_2*S7;
+  FM33_2 = FB33_2-FM110_2*S10-FM14_2*S4-FM17_2*S7+FM310_2*C10+FM34_2*C4+FM37_2*C7;
+  CM23_2 = CM210_2+CM24_2+CM27_2+FB13_2*l[3][3]-FB33_2*l[1][3]-dpt[1][3]*(-FM110_2*S10+FM310_2*C10)+dpt[3][3]*(FM110_2*
+ C10+FM310_2*S10);
+  FB13_3 = m[3]*l[3][3];
+  FB33_3 = -m[3]*l[1][3];
+  CM23_3 = In[5][3]+CM210_3+CM24_3+CM27_3+FB13_3*l[3][3]-FB33_3*l[1][3]-dpt[1][3]*(-FM110_3*S10+FM310_3*C10)+dpt[3][3]*(
+ FM110_3*C10+FM310_3*S10);
+  FF12 = FF13*C3+FF33*S3;
+  FF32 = -FF13*S3+FF33*C3;
+  FM12_1 = FM13_1*C3+FM33_1*S3;
+  FM32_1 = -FM13_1*S3+FM33_1*C3;
+  FM32_2 = -FM13_2*S3+FM33_2*C3;
+ 
+// Reduction (Coordinate Partitioning)
+
+  Fr1 = FF12-Qq[1];
+  Fr2 = FF32-Qq[2];
+  Fr3 = CF23-Qq[3];
+  Fr4 = CF24-Qq[4];
+  Fr5 = CF25-Qq[5];
+  Fr6 = CF26-Qq[6];
+  Fr7 = CF27-Qq[7];
+  Fr8 = CF28-Qq[8];
+  Fr9 = CF29-Qq[9];
+  Fr10 = CF210-Qq[10];
+  Fr11 = CF211-Qq[11];
+  Fr12 = CF212-Qq[12];
+ 
+// Linear System Resolution
+
+  Mrlu2_1 = FM32_1/FM12_1;
+  Mrlu2_2 = FM32_2-FM32_1*Mrlu2_1;
+  Mrlu2_3 = CM23_2-CM23_1*Mrlu2_1;
+  Mrlu2_4 = CM24_2-CM24_1*Mrlu2_1;
+  Mrlu2_5 = CM25_2-CM25_1*Mrlu2_1;
+  Mrlu2_6 = CM26_2-CM26_1*Mrlu2_1;
+  Mrlu2_7 = CM27_2-CM27_1*Mrlu2_1;
+  Mrlu2_8 = CM28_2-CM28_1*Mrlu2_1;
+  Mrlu2_9 = CM29_2-CM29_1*Mrlu2_1;
+  Mrlu2_10 = CM210_2-CM210_1*Mrlu2_1;
+  Mrlu2_11 = CM211_2-CM211_1*Mrlu2_1;
+  Mrlu2_12 = CM212_2-CM212_1*Mrlu2_1;
+  Mrlu3_1 = CM23_1/FM12_1;
+  Mrlu1_3_2 = CM23_2-FM32_1*Mrlu3_1;
+  Mrlu1_3_3 = CM23_3-CM23_1*Mrlu3_1;
+  Mrlu1_3_4 = CM24_3-CM24_1*Mrlu3_1;
+  Mrlu1_3_5 = CM25_3-CM25_1*Mrlu3_1;
+  Mrlu1_3_6 = CM26_3-CM26_1*Mrlu3_1;
+  Mrlu1_3_7 = CM27_3-CM27_1*Mrlu3_1;
+  Mrlu1_3_8 = CM28_3-CM28_1*Mrlu3_1;
+  Mrlu1_3_9 = CM29_3-CM29_1*Mrlu3_1;
+  Mrlu1_3_10 = CM210_3-CM210_1*Mrlu3_1;
+  Mrlu1_3_11 = CM211_3-CM211_1*Mrlu3_1;
+  Mrlu1_3_12 = CM212_3-CM212_1*Mrlu3_1;
+  Mrlu4_1 = CM24_1/FM12_1;
+  Mrlu1_4_2 = CM24_2-FM32_1*Mrlu4_1;
+  Mrlu1_4_3 = CM24_3-CM23_1*Mrlu4_1;
+  Mrlu1_4_4 = CM24_4-CM24_1*Mrlu4_1;
+  Mrlu1_4_5 = CM25_4-CM25_1*Mrlu4_1;
+  Mrlu1_4_6 = CM26_4-CM26_1*Mrlu4_1;
+  Mrlu1_4_7 = -CM27_1*Mrlu4_1;
+  Mrlu1_4_8 = -CM28_1*Mrlu4_1;
+  Mrlu1_4_9 = -CM29_1*Mrlu4_1;
+  Mrlu1_4_10 = -CM210_1*Mrlu4_1;
+  Mrlu1_4_11 = -CM211_1*Mrlu4_1;
+  Mrlu1_4_12 = -CM212_1*Mrlu4_1;
+  Mrlu5_1 = CM25_1/FM12_1;
+  Mrlu1_5_2 = CM25_2-FM32_1*Mrlu5_1;
+  Mrlu1_5_3 = CM25_3-CM23_1*Mrlu5_1;
+  Mrlu1_5_4 = CM25_4-CM24_1*Mrlu5_1;
+  Mrlu1_5_5 = CM25_5-CM25_1*Mrlu5_1;
+  Mrlu1_5_6 = CM26_5-CM26_1*Mrlu5_1;
+  Mrlu1_5_7 = -CM27_1*Mrlu5_1;
+  Mrlu1_5_8 = -CM28_1*Mrlu5_1;
+  Mrlu1_5_9 = -CM29_1*Mrlu5_1;
+  Mrlu1_5_10 = -CM210_1*Mrlu5_1;
+  Mrlu1_5_11 = -CM211_1*Mrlu5_1;
+  Mrlu1_5_12 = -CM212_1*Mrlu5_1;
+  Mrlu6_1 = CM26_1/FM12_1;
+  Mrlu1_6_2 = CM26_2-FM32_1*Mrlu6_1;
+  Mrlu1_6_3 = CM26_3-CM23_1*Mrlu6_1;
+  Mrlu1_6_4 = CM26_4-CM24_1*Mrlu6_1;
+  Mrlu1_6_5 = CM26_5-CM25_1*Mrlu6_1;
+  Mrlu1_6_6 = CM26_6-CM26_1*Mrlu6_1;
+  Mrlu1_6_7 = -CM27_1*Mrlu6_1;
+  Mrlu1_6_8 = -CM28_1*Mrlu6_1;
+  Mrlu1_6_9 = -CM29_1*Mrlu6_1;
+  Mrlu1_6_10 = -CM210_1*Mrlu6_1;
+  Mrlu1_6_11 = -CM211_1*Mrlu6_1;
+  Mrlu1_6_12 = -CM212_1*Mrlu6_1;
+  Mrlu7_1 = CM27_1/FM12_1;
+  Mrlu1_7_2 = CM27_2-FM32_1*Mrlu7_1;
+  Mrlu1_7_3 = CM27_3-CM23_1*Mrlu7_1;
+  Mrlu1_7_4 = -CM24_1*Mrlu7_1;
+  Mrlu1_7_5 = -CM25_1*Mrlu7_1;
+  Mrlu1_7_6 = -CM26_1*Mrlu7_1;
+  Mrlu1_7_7 = CM27_7-CM27_1*Mrlu7_1;
+  Mrlu1_7_8 = CM28_7-CM28_1*Mrlu7_1;
+  Mrlu1_7_9 = CM29_7-CM29_1*Mrlu7_1;
+  Mrlu1_7_10 = -CM210_1*Mrlu7_1;
+  Mrlu1_7_11 = -CM211_1*Mrlu7_1;
+  Mrlu1_7_12 = -CM212_1*Mrlu7_1;
+  Mrlu8_1 = CM28_1/FM12_1;
+  Mrlu1_8_2 = CM28_2-FM32_1*Mrlu8_1;
+  Mrlu1_8_3 = CM28_3-CM23_1*Mrlu8_1;
+  Mrlu1_8_4 = -CM24_1*Mrlu8_1;
+  Mrlu1_8_5 = -CM25_1*Mrlu8_1;
+  Mrlu1_8_6 = -CM26_1*Mrlu8_1;
+  Mrlu1_8_7 = CM28_7-CM27_1*Mrlu8_1;
+  Mrlu1_8_8 = CM28_8-CM28_1*Mrlu8_1;
+  Mrlu1_8_9 = CM29_8-CM29_1*Mrlu8_1;
+  Mrlu1_8_10 = -CM210_1*Mrlu8_1;
+  Mrlu1_8_11 = -CM211_1*Mrlu8_1;
+  Mrlu1_8_12 = -CM212_1*Mrlu8_1;
+  Mrlu9_1 = CM29_1/FM12_1;
+  Mrlu1_9_2 = CM29_2-FM32_1*Mrlu9_1;
+  Mrlu1_9_3 = CM29_3-CM23_1*Mrlu9_1;
+  Mrlu1_9_4 = -CM24_1*Mrlu9_1;
+  Mrlu1_9_5 = -CM25_1*Mrlu9_1;
+  Mrlu1_9_6 = -CM26_1*Mrlu9_1;
+  Mrlu1_9_7 = CM29_7-CM27_1*Mrlu9_1;
+  Mrlu1_9_8 = CM29_8-CM28_1*Mrlu9_1;
+  Mrlu1_9_9 = CM29_9-CM29_1*Mrlu9_1;
+  Mrlu1_9_10 = -CM210_1*Mrlu9_1;
+  Mrlu1_9_11 = -CM211_1*Mrlu9_1;
+  Mrlu1_9_12 = -CM212_1*Mrlu9_1;
+  Mrlu10_1 = CM210_1/FM12_1;
+  Mrlu1_10_2 = CM210_2-FM32_1*Mrlu10_1;
+  Mrlu1_10_3 = CM210_3-CM23_1*Mrlu10_1;
+  Mrlu1_10_4 = -CM24_1*Mrlu10_1;
+  Mrlu1_10_5 = -CM25_1*Mrlu10_1;
+  Mrlu1_10_6 = -CM26_1*Mrlu10_1;
+  Mrlu1_10_7 = -CM27_1*Mrlu10_1;
+  Mrlu1_10_8 = -CM28_1*Mrlu10_1;
+  Mrlu1_10_9 = -CM29_1*Mrlu10_1;
+  Mrlu1_10_10 = CM210_10-CM210_1*Mrlu10_1;
+  Mrlu1_10_11 = CM211_10-CM211_1*Mrlu10_1;
+  Mrlu1_10_12 = CM212_10-CM212_1*Mrlu10_1;
+  Mrlu11_1 = CM211_1/FM12_1;
+  Mrlu1_11_2 = CM211_2-FM32_1*Mrlu11_1;
+  Mrlu1_11_3 = CM211_3-CM23_1*Mrlu11_1;
+  Mrlu1_11_4 = -CM24_1*Mrlu11_1;
+  Mrlu1_11_5 = -CM25_1*Mrlu11_1;
+  Mrlu1_11_6 = -CM26_1*Mrlu11_1;
+  Mrlu1_11_7 = -CM27_1*Mrlu11_1;
+  Mrlu1_11_8 = -CM28_1*Mrlu11_1;
+  Mrlu1_11_9 = -CM29_1*Mrlu11_1;
+  Mrlu1_11_10 = CM211_10-CM210_1*Mrlu11_1;
+  Mrlu1_11_11 = CM211_11-CM211_1*Mrlu11_1;
+  Mrlu1_11_12 = -CM212_1*Mrlu11_1;
+  Mrlu12_1 = CM212_1/FM12_1;
+  Mrlu1_12_2 = CM212_2-FM32_1*Mrlu12_1;
+  Mrlu1_12_3 = CM212_3-CM23_1*Mrlu12_1;
+  Mrlu1_12_4 = -CM24_1*Mrlu12_1;
+  Mrlu1_12_5 = -CM25_1*Mrlu12_1;
+  Mrlu1_12_6 = -CM26_1*Mrlu12_1;
+  Mrlu1_12_7 = -CM27_1*Mrlu12_1;
+  Mrlu1_12_8 = -CM28_1*Mrlu12_1;
+  Mrlu1_12_9 = -CM29_1*Mrlu12_1;
+  Mrlu1_12_10 = CM212_10-CM210_1*Mrlu12_1;
+  Mrlu1_12_11 = -CM211_1*Mrlu12_1;
+  Mrlu1_12_12 = CM212_12-CM212_1*Mrlu12_1;
+  Mrlu3_2 = Mrlu1_3_2/Mrlu2_2;
+  Mrlu3_3 = Mrlu1_3_3-Mrlu2_3*Mrlu3_2;
+  Mrlu3_4 = Mrlu1_3_4-Mrlu2_4*Mrlu3_2;
+  Mrlu3_5 = Mrlu1_3_5-Mrlu2_5*Mrlu3_2;
+  Mrlu3_6 = Mrlu1_3_6-Mrlu2_6*Mrlu3_2;
+  Mrlu3_7 = Mrlu1_3_7-Mrlu2_7*Mrlu3_2;
+  Mrlu3_8 = Mrlu1_3_8-Mrlu2_8*Mrlu3_2;
+  Mrlu3_9 = Mrlu1_3_9-Mrlu2_9*Mrlu3_2;
+  Mrlu3_10 = Mrlu1_3_10-Mrlu2_10*Mrlu3_2;
+  Mrlu3_11 = Mrlu1_3_11-Mrlu2_11*Mrlu3_2;
+  Mrlu3_12 = Mrlu1_3_12-Mrlu2_12*Mrlu3_2;
+  Mrlu4_2 = Mrlu1_4_2/Mrlu2_2;
+  Mrlu2_4_3 = Mrlu1_4_3-Mrlu2_3*Mrlu4_2;
+  Mrlu2_4_4 = Mrlu1_4_4-Mrlu2_4*Mrlu4_2;
+  Mrlu2_4_5 = Mrlu1_4_5-Mrlu2_5*Mrlu4_2;
+  Mrlu2_4_6 = Mrlu1_4_6-Mrlu2_6*Mrlu4_2;
+  Mrlu2_4_7 = Mrlu1_4_7-Mrlu2_7*Mrlu4_2;
+  Mrlu2_4_8 = Mrlu1_4_8-Mrlu2_8*Mrlu4_2;
+  Mrlu2_4_9 = Mrlu1_4_9-Mrlu2_9*Mrlu4_2;
+  Mrlu2_4_10 = Mrlu1_4_10-Mrlu2_10*Mrlu4_2;
+  Mrlu2_4_11 = Mrlu1_4_11-Mrlu2_11*Mrlu4_2;
+  Mrlu2_4_12 = Mrlu1_4_12-Mrlu2_12*Mrlu4_2;
+  Mrlu5_2 = Mrlu1_5_2/Mrlu2_2;
+  Mrlu2_5_3 = Mrlu1_5_3-Mrlu2_3*Mrlu5_2;
+  Mrlu2_5_4 = Mrlu1_5_4-Mrlu2_4*Mrlu5_2;
+  Mrlu2_5_5 = Mrlu1_5_5-Mrlu2_5*Mrlu5_2;
+  Mrlu2_5_6 = Mrlu1_5_6-Mrlu2_6*Mrlu5_2;
+  Mrlu2_5_7 = Mrlu1_5_7-Mrlu2_7*Mrlu5_2;
+  Mrlu2_5_8 = Mrlu1_5_8-Mrlu2_8*Mrlu5_2;
+  Mrlu2_5_9 = Mrlu1_5_9-Mrlu2_9*Mrlu5_2;
+  Mrlu2_5_10 = Mrlu1_5_10-Mrlu2_10*Mrlu5_2;
+  Mrlu2_5_11 = Mrlu1_5_11-Mrlu2_11*Mrlu5_2;
+  Mrlu2_5_12 = Mrlu1_5_12-Mrlu2_12*Mrlu5_2;
+  Mrlu6_2 = Mrlu1_6_2/Mrlu2_2;
+  Mrlu2_6_3 = Mrlu1_6_3-Mrlu2_3*Mrlu6_2;
+  Mrlu2_6_4 = Mrlu1_6_4-Mrlu2_4*Mrlu6_2;
+  Mrlu2_6_5 = Mrlu1_6_5-Mrlu2_5*Mrlu6_2;
+  Mrlu2_6_6 = Mrlu1_6_6-Mrlu2_6*Mrlu6_2;
+  Mrlu2_6_7 = Mrlu1_6_7-Mrlu2_7*Mrlu6_2;
+  Mrlu2_6_8 = Mrlu1_6_8-Mrlu2_8*Mrlu6_2;
+  Mrlu2_6_9 = Mrlu1_6_9-Mrlu2_9*Mrlu6_2;
+  Mrlu2_6_10 = Mrlu1_6_10-Mrlu2_10*Mrlu6_2;
+  Mrlu2_6_11 = Mrlu1_6_11-Mrlu2_11*Mrlu6_2;
+  Mrlu2_6_12 = Mrlu1_6_12-Mrlu2_12*Mrlu6_2;
+  Mrlu7_2 = Mrlu1_7_2/Mrlu2_2;
+  Mrlu2_7_3 = Mrlu1_7_3-Mrlu2_3*Mrlu7_2;
+  Mrlu2_7_4 = Mrlu1_7_4-Mrlu2_4*Mrlu7_2;
+  Mrlu2_7_5 = Mrlu1_7_5-Mrlu2_5*Mrlu7_2;
+  Mrlu2_7_6 = Mrlu1_7_6-Mrlu2_6*Mrlu7_2;
+  Mrlu2_7_7 = Mrlu1_7_7-Mrlu2_7*Mrlu7_2;
+  Mrlu2_7_8 = Mrlu1_7_8-Mrlu2_8*Mrlu7_2;
+  Mrlu2_7_9 = Mrlu1_7_9-Mrlu2_9*Mrlu7_2;
+  Mrlu2_7_10 = Mrlu1_7_10-Mrlu2_10*Mrlu7_2;
+  Mrlu2_7_11 = Mrlu1_7_11-Mrlu2_11*Mrlu7_2;
+  Mrlu2_7_12 = Mrlu1_7_12-Mrlu2_12*Mrlu7_2;
+  Mrlu8_2 = Mrlu1_8_2/Mrlu2_2;
+  Mrlu2_8_3 = Mrlu1_8_3-Mrlu2_3*Mrlu8_2;
+  Mrlu2_8_4 = Mrlu1_8_4-Mrlu2_4*Mrlu8_2;
+  Mrlu2_8_5 = Mrlu1_8_5-Mrlu2_5*Mrlu8_2;
+  Mrlu2_8_6 = Mrlu1_8_6-Mrlu2_6*Mrlu8_2;
+  Mrlu2_8_7 = Mrlu1_8_7-Mrlu2_7*Mrlu8_2;
+  Mrlu2_8_8 = Mrlu1_8_8-Mrlu2_8*Mrlu8_2;
+  Mrlu2_8_9 = Mrlu1_8_9-Mrlu2_9*Mrlu8_2;
+  Mrlu2_8_10 = Mrlu1_8_10-Mrlu2_10*Mrlu8_2;
+  Mrlu2_8_11 = Mrlu1_8_11-Mrlu2_11*Mrlu8_2;
+  Mrlu2_8_12 = Mrlu1_8_12-Mrlu2_12*Mrlu8_2;
+  Mrlu9_2 = Mrlu1_9_2/Mrlu2_2;
+  Mrlu2_9_3 = Mrlu1_9_3-Mrlu2_3*Mrlu9_2;
+  Mrlu2_9_4 = Mrlu1_9_4-Mrlu2_4*Mrlu9_2;
+  Mrlu2_9_5 = Mrlu1_9_5-Mrlu2_5*Mrlu9_2;
+  Mrlu2_9_6 = Mrlu1_9_6-Mrlu2_6*Mrlu9_2;
+  Mrlu2_9_7 = Mrlu1_9_7-Mrlu2_7*Mrlu9_2;
+  Mrlu2_9_8 = Mrlu1_9_8-Mrlu2_8*Mrlu9_2;
+  Mrlu2_9_9 = Mrlu1_9_9-Mrlu2_9*Mrlu9_2;
+  Mrlu2_9_10 = Mrlu1_9_10-Mrlu2_10*Mrlu9_2;
+  Mrlu2_9_11 = Mrlu1_9_11-Mrlu2_11*Mrlu9_2;
+  Mrlu2_9_12 = Mrlu1_9_12-Mrlu2_12*Mrlu9_2;
+  Mrlu10_2 = Mrlu1_10_2/Mrlu2_2;
+  Mrlu2_10_3 = Mrlu1_10_3-Mrlu10_2*Mrlu2_3;
+  Mrlu2_10_4 = Mrlu1_10_4-Mrlu10_2*Mrlu2_4;
+  Mrlu2_10_5 = Mrlu1_10_5-Mrlu10_2*Mrlu2_5;
+  Mrlu2_10_6 = Mrlu1_10_6-Mrlu10_2*Mrlu2_6;
+  Mrlu2_10_7 = Mrlu1_10_7-Mrlu10_2*Mrlu2_7;
+  Mrlu2_10_8 = Mrlu1_10_8-Mrlu10_2*Mrlu2_8;
+  Mrlu2_10_9 = Mrlu1_10_9-Mrlu10_2*Mrlu2_9;
+  Mrlu2_10_10 = Mrlu1_10_10-Mrlu10_2*Mrlu2_10;
+  Mrlu2_10_11 = Mrlu1_10_11-Mrlu10_2*Mrlu2_11;
+  Mrlu2_10_12 = Mrlu1_10_12-Mrlu10_2*Mrlu2_12;
+  Mrlu11_2 = Mrlu1_11_2/Mrlu2_2;
+  Mrlu2_11_3 = Mrlu1_11_3-Mrlu11_2*Mrlu2_3;
+  Mrlu2_11_4 = Mrlu1_11_4-Mrlu11_2*Mrlu2_4;
+  Mrlu2_11_5 = Mrlu1_11_5-Mrlu11_2*Mrlu2_5;
+  Mrlu2_11_6 = Mrlu1_11_6-Mrlu11_2*Mrlu2_6;
+  Mrlu2_11_7 = Mrlu1_11_7-Mrlu11_2*Mrlu2_7;
+  Mrlu2_11_8 = Mrlu1_11_8-Mrlu11_2*Mrlu2_8;
+  Mrlu2_11_9 = Mrlu1_11_9-Mrlu11_2*Mrlu2_9;
+  Mrlu2_11_10 = Mrlu1_11_10-Mrlu11_2*Mrlu2_10;
+  Mrlu2_11_11 = Mrlu1_11_11-Mrlu11_2*Mrlu2_11;
+  Mrlu2_11_12 = Mrlu1_11_12-Mrlu11_2*Mrlu2_12;
+  Mrlu12_2 = Mrlu1_12_2/Mrlu2_2;
+  Mrlu2_12_3 = Mrlu1_12_3-Mrlu12_2*Mrlu2_3;
+  Mrlu2_12_4 = Mrlu1_12_4-Mrlu12_2*Mrlu2_4;
+  Mrlu2_12_5 = Mrlu1_12_5-Mrlu12_2*Mrlu2_5;
+  Mrlu2_12_6 = Mrlu1_12_6-Mrlu12_2*Mrlu2_6;
+  Mrlu2_12_7 = Mrlu1_12_7-Mrlu12_2*Mrlu2_7;
+  Mrlu2_12_8 = Mrlu1_12_8-Mrlu12_2*Mrlu2_8;
+  Mrlu2_12_9 = Mrlu1_12_9-Mrlu12_2*Mrlu2_9;
+  Mrlu2_12_10 = Mrlu1_12_10-Mrlu12_2*Mrlu2_10;
+  Mrlu2_12_11 = Mrlu1_12_11-Mrlu12_2*Mrlu2_11;
+  Mrlu2_12_12 = Mrlu1_12_12-Mrlu12_2*Mrlu2_12;
+  Mrlu4_3 = Mrlu2_4_3/Mrlu3_3;
+  Mrlu4_4 = Mrlu2_4_4-Mrlu3_4*Mrlu4_3;
+  Mrlu4_5 = Mrlu2_4_5-Mrlu3_5*Mrlu4_3;
+  Mrlu4_6 = Mrlu2_4_6-Mrlu3_6*Mrlu4_3;
+  Mrlu4_7 = Mrlu2_4_7-Mrlu3_7*Mrlu4_3;
+  Mrlu4_8 = Mrlu2_4_8-Mrlu3_8*Mrlu4_3;
+  Mrlu4_9 = Mrlu2_4_9-Mrlu3_9*Mrlu4_3;
+  Mrlu4_10 = Mrlu2_4_10-Mrlu3_10*Mrlu4_3;
+  Mrlu4_11 = Mrlu2_4_11-Mrlu3_11*Mrlu4_3;
+  Mrlu4_12 = Mrlu2_4_12-Mrlu3_12*Mrlu4_3;
+  Mrlu5_3 = Mrlu2_5_3/Mrlu3_3;
+  Mrlu3_5_4 = Mrlu2_5_4-Mrlu3_4*Mrlu5_3;
+  Mrlu3_5_5 = Mrlu2_5_5-Mrlu3_5*Mrlu5_3;
+  Mrlu3_5_6 = Mrlu2_5_6-Mrlu3_6*Mrlu5_3;
+  Mrlu3_5_7 = Mrlu2_5_7-Mrlu3_7*Mrlu5_3;
+  Mrlu3_5_8 = Mrlu2_5_8-Mrlu3_8*Mrlu5_3;
+  Mrlu3_5_9 = Mrlu2_5_9-Mrlu3_9*Mrlu5_3;
+  Mrlu3_5_10 = Mrlu2_5_10-Mrlu3_10*Mrlu5_3;
+  Mrlu3_5_11 = Mrlu2_5_11-Mrlu3_11*Mrlu5_3;
+  Mrlu3_5_12 = Mrlu2_5_12-Mrlu3_12*Mrlu5_3;
+  Mrlu6_3 = Mrlu2_6_3/Mrlu3_3;
+  Mrlu3_6_4 = Mrlu2_6_4-Mrlu3_4*Mrlu6_3;
+  Mrlu3_6_5 = Mrlu2_6_5-Mrlu3_5*Mrlu6_3;
+  Mrlu3_6_6 = Mrlu2_6_6-Mrlu3_6*Mrlu6_3;
+  Mrlu3_6_7 = Mrlu2_6_7-Mrlu3_7*Mrlu6_3;
+  Mrlu3_6_8 = Mrlu2_6_8-Mrlu3_8*Mrlu6_3;
+  Mrlu3_6_9 = Mrlu2_6_9-Mrlu3_9*Mrlu6_3;
+  Mrlu3_6_10 = Mrlu2_6_10-Mrlu3_10*Mrlu6_3;
+  Mrlu3_6_11 = Mrlu2_6_11-Mrlu3_11*Mrlu6_3;
+  Mrlu3_6_12 = Mrlu2_6_12-Mrlu3_12*Mrlu6_3;
+  Mrlu7_3 = Mrlu2_7_3/Mrlu3_3;
+  Mrlu3_7_4 = Mrlu2_7_4-Mrlu3_4*Mrlu7_3;
+  Mrlu3_7_5 = Mrlu2_7_5-Mrlu3_5*Mrlu7_3;
+  Mrlu3_7_6 = Mrlu2_7_6-Mrlu3_6*Mrlu7_3;
+  Mrlu3_7_7 = Mrlu2_7_7-Mrlu3_7*Mrlu7_3;
+  Mrlu3_7_8 = Mrlu2_7_8-Mrlu3_8*Mrlu7_3;
+  Mrlu3_7_9 = Mrlu2_7_9-Mrlu3_9*Mrlu7_3;
+  Mrlu3_7_10 = Mrlu2_7_10-Mrlu3_10*Mrlu7_3;
+  Mrlu3_7_11 = Mrlu2_7_11-Mrlu3_11*Mrlu7_3;
+  Mrlu3_7_12 = Mrlu2_7_12-Mrlu3_12*Mrlu7_3;
+  Mrlu8_3 = Mrlu2_8_3/Mrlu3_3;
+  Mrlu3_8_4 = Mrlu2_8_4-Mrlu3_4*Mrlu8_3;
+  Mrlu3_8_5 = Mrlu2_8_5-Mrlu3_5*Mrlu8_3;
+  Mrlu3_8_6 = Mrlu2_8_6-Mrlu3_6*Mrlu8_3;
+  Mrlu3_8_7 = Mrlu2_8_7-Mrlu3_7*Mrlu8_3;
+  Mrlu3_8_8 = Mrlu2_8_8-Mrlu3_8*Mrlu8_3;
+  Mrlu3_8_9 = Mrlu2_8_9-Mrlu3_9*Mrlu8_3;
+  Mrlu3_8_10 = Mrlu2_8_10-Mrlu3_10*Mrlu8_3;
+  Mrlu3_8_11 = Mrlu2_8_11-Mrlu3_11*Mrlu8_3;
+  Mrlu3_8_12 = Mrlu2_8_12-Mrlu3_12*Mrlu8_3;
+  Mrlu9_3 = Mrlu2_9_3/Mrlu3_3;
+  Mrlu3_9_4 = Mrlu2_9_4-Mrlu3_4*Mrlu9_3;
+  Mrlu3_9_5 = Mrlu2_9_5-Mrlu3_5*Mrlu9_3;
+  Mrlu3_9_6 = Mrlu2_9_6-Mrlu3_6*Mrlu9_3;
+  Mrlu3_9_7 = Mrlu2_9_7-Mrlu3_7*Mrlu9_3;
+  Mrlu3_9_8 = Mrlu2_9_8-Mrlu3_8*Mrlu9_3;
+  Mrlu3_9_9 = Mrlu2_9_9-Mrlu3_9*Mrlu9_3;
+  Mrlu3_9_10 = Mrlu2_9_10-Mrlu3_10*Mrlu9_3;
+  Mrlu3_9_11 = Mrlu2_9_11-Mrlu3_11*Mrlu9_3;
+  Mrlu3_9_12 = Mrlu2_9_12-Mrlu3_12*Mrlu9_3;
+  Mrlu10_3 = Mrlu2_10_3/Mrlu3_3;
+  Mrlu3_10_4 = Mrlu2_10_4-Mrlu10_3*Mrlu3_4;
+  Mrlu3_10_5 = Mrlu2_10_5-Mrlu10_3*Mrlu3_5;
+  Mrlu3_10_6 = Mrlu2_10_6-Mrlu10_3*Mrlu3_6;
+  Mrlu3_10_7 = Mrlu2_10_7-Mrlu10_3*Mrlu3_7;
+  Mrlu3_10_8 = Mrlu2_10_8-Mrlu10_3*Mrlu3_8;
+  Mrlu3_10_9 = Mrlu2_10_9-Mrlu10_3*Mrlu3_9;
+  Mrlu3_10_10 = Mrlu2_10_10-Mrlu10_3*Mrlu3_10;
+  Mrlu3_10_11 = Mrlu2_10_11-Mrlu10_3*Mrlu3_11;
+  Mrlu3_10_12 = Mrlu2_10_12-Mrlu10_3*Mrlu3_12;
+  Mrlu11_3 = Mrlu2_11_3/Mrlu3_3;
+  Mrlu3_11_4 = Mrlu2_11_4-Mrlu11_3*Mrlu3_4;
+  Mrlu3_11_5 = Mrlu2_11_5-Mrlu11_3*Mrlu3_5;
+  Mrlu3_11_6 = Mrlu2_11_6-Mrlu11_3*Mrlu3_6;
+  Mrlu3_11_7 = Mrlu2_11_7-Mrlu11_3*Mrlu3_7;
+  Mrlu3_11_8 = Mrlu2_11_8-Mrlu11_3*Mrlu3_8;
+  Mrlu3_11_9 = Mrlu2_11_9-Mrlu11_3*Mrlu3_9;
+  Mrlu3_11_10 = Mrlu2_11_10-Mrlu11_3*Mrlu3_10;
+  Mrlu3_11_11 = Mrlu2_11_11-Mrlu11_3*Mrlu3_11;
+  Mrlu3_11_12 = Mrlu2_11_12-Mrlu11_3*Mrlu3_12;
+  Mrlu12_3 = Mrlu2_12_3/Mrlu3_3;
+  Mrlu3_12_4 = Mrlu2_12_4-Mrlu12_3*Mrlu3_4;
+  Mrlu3_12_5 = Mrlu2_12_5-Mrlu12_3*Mrlu3_5;
+  Mrlu3_12_6 = Mrlu2_12_6-Mrlu12_3*Mrlu3_6;
+  Mrlu3_12_7 = Mrlu2_12_7-Mrlu12_3*Mrlu3_7;
+  Mrlu3_12_8 = Mrlu2_12_8-Mrlu12_3*Mrlu3_8;
+  Mrlu3_12_9 = Mrlu2_12_9-Mrlu12_3*Mrlu3_9;
+  Mrlu3_12_10 = Mrlu2_12_10-Mrlu12_3*Mrlu3_10;
+  Mrlu3_12_11 = Mrlu2_12_11-Mrlu12_3*Mrlu3_11;
+  Mrlu3_12_12 = Mrlu2_12_12-Mrlu12_3*Mrlu3_12;
+  Mrlu5_4 = Mrlu3_5_4/Mrlu4_4;
+  Mrlu5_5 = Mrlu3_5_5-Mrlu4_5*Mrlu5_4;
+  Mrlu5_6 = Mrlu3_5_6-Mrlu4_6*Mrlu5_4;
+  Mrlu5_7 = Mrlu3_5_7-Mrlu4_7*Mrlu5_4;
+  Mrlu5_8 = Mrlu3_5_8-Mrlu4_8*Mrlu5_4;
+  Mrlu5_9 = Mrlu3_5_9-Mrlu4_9*Mrlu5_4;
+  Mrlu5_10 = Mrlu3_5_10-Mrlu4_10*Mrlu5_4;
+  Mrlu5_11 = Mrlu3_5_11-Mrlu4_11*Mrlu5_4;
+  Mrlu5_12 = Mrlu3_5_12-Mrlu4_12*Mrlu5_4;
+  Mrlu6_4 = Mrlu3_6_4/Mrlu4_4;
+  Mrlu4_6_5 = Mrlu3_6_5-Mrlu4_5*Mrlu6_4;
+  Mrlu4_6_6 = Mrlu3_6_6-Mrlu4_6*Mrlu6_4;
+  Mrlu4_6_7 = Mrlu3_6_7-Mrlu4_7*Mrlu6_4;
+  Mrlu4_6_8 = Mrlu3_6_8-Mrlu4_8*Mrlu6_4;
+  Mrlu4_6_9 = Mrlu3_6_9-Mrlu4_9*Mrlu6_4;
+  Mrlu4_6_10 = Mrlu3_6_10-Mrlu4_10*Mrlu6_4;
+  Mrlu4_6_11 = Mrlu3_6_11-Mrlu4_11*Mrlu6_4;
+  Mrlu4_6_12 = Mrlu3_6_12-Mrlu4_12*Mrlu6_4;
+  Mrlu7_4 = Mrlu3_7_4/Mrlu4_4;
+  Mrlu4_7_5 = Mrlu3_7_5-Mrlu4_5*Mrlu7_4;
+  Mrlu4_7_6 = Mrlu3_7_6-Mrlu4_6*Mrlu7_4;
+  Mrlu4_7_7 = Mrlu3_7_7-Mrlu4_7*Mrlu7_4;
+  Mrlu4_7_8 = Mrlu3_7_8-Mrlu4_8*Mrlu7_4;
+  Mrlu4_7_9 = Mrlu3_7_9-Mrlu4_9*Mrlu7_4;
+  Mrlu4_7_10 = Mrlu3_7_10-Mrlu4_10*Mrlu7_4;
+  Mrlu4_7_11 = Mrlu3_7_11-Mrlu4_11*Mrlu7_4;
+  Mrlu4_7_12 = Mrlu3_7_12-Mrlu4_12*Mrlu7_4;
+  Mrlu8_4 = Mrlu3_8_4/Mrlu4_4;
+  Mrlu4_8_5 = Mrlu3_8_5-Mrlu4_5*Mrlu8_4;
+  Mrlu4_8_6 = Mrlu3_8_6-Mrlu4_6*Mrlu8_4;
+  Mrlu4_8_7 = Mrlu3_8_7-Mrlu4_7*Mrlu8_4;
+  Mrlu4_8_8 = Mrlu3_8_8-Mrlu4_8*Mrlu8_4;
+  Mrlu4_8_9 = Mrlu3_8_9-Mrlu4_9*Mrlu8_4;
+  Mrlu4_8_10 = Mrlu3_8_10-Mrlu4_10*Mrlu8_4;
+  Mrlu4_8_11 = Mrlu3_8_11-Mrlu4_11*Mrlu8_4;
+  Mrlu4_8_12 = Mrlu3_8_12-Mrlu4_12*Mrlu8_4;
+  Mrlu9_4 = Mrlu3_9_4/Mrlu4_4;
+  Mrlu4_9_5 = Mrlu3_9_5-Mrlu4_5*Mrlu9_4;
+  Mrlu4_9_6 = Mrlu3_9_6-Mrlu4_6*Mrlu9_4;
+  Mrlu4_9_7 = Mrlu3_9_7-Mrlu4_7*Mrlu9_4;
+  Mrlu4_9_8 = Mrlu3_9_8-Mrlu4_8*Mrlu9_4;
+  Mrlu4_9_9 = Mrlu3_9_9-Mrlu4_9*Mrlu9_4;
+  Mrlu4_9_10 = Mrlu3_9_10-Mrlu4_10*Mrlu9_4;
+  Mrlu4_9_11 = Mrlu3_9_11-Mrlu4_11*Mrlu9_4;
+  Mrlu4_9_12 = Mrlu3_9_12-Mrlu4_12*Mrlu9_4;
+  Mrlu10_4 = Mrlu3_10_4/Mrlu4_4;
+  Mrlu4_10_5 = Mrlu3_10_5-Mrlu10_4*Mrlu4_5;
+  Mrlu4_10_6 = Mrlu3_10_6-Mrlu10_4*Mrlu4_6;
+  Mrlu4_10_7 = Mrlu3_10_7-Mrlu10_4*Mrlu4_7;
+  Mrlu4_10_8 = Mrlu3_10_8-Mrlu10_4*Mrlu4_8;
+  Mrlu4_10_9 = Mrlu3_10_9-Mrlu10_4*Mrlu4_9;
+  Mrlu4_10_10 = Mrlu3_10_10-Mrlu10_4*Mrlu4_10;
+  Mrlu4_10_11 = Mrlu3_10_11-Mrlu10_4*Mrlu4_11;
+  Mrlu4_10_12 = Mrlu3_10_12-Mrlu10_4*Mrlu4_12;
+  Mrlu11_4 = Mrlu3_11_4/Mrlu4_4;
+  Mrlu4_11_5 = Mrlu3_11_5-Mrlu11_4*Mrlu4_5;
+  Mrlu4_11_6 = Mrlu3_11_6-Mrlu11_4*Mrlu4_6;
+  Mrlu4_11_7 = Mrlu3_11_7-Mrlu11_4*Mrlu4_7;
+  Mrlu4_11_8 = Mrlu3_11_8-Mrlu11_4*Mrlu4_8;
+  Mrlu4_11_9 = Mrlu3_11_9-Mrlu11_4*Mrlu4_9;
+  Mrlu4_11_10 = Mrlu3_11_10-Mrlu11_4*Mrlu4_10;
+  Mrlu4_11_11 = Mrlu3_11_11-Mrlu11_4*Mrlu4_11;
+  Mrlu4_11_12 = Mrlu3_11_12-Mrlu11_4*Mrlu4_12;
+  Mrlu12_4 = Mrlu3_12_4/Mrlu4_4;
+  Mrlu4_12_5 = Mrlu3_12_5-Mrlu12_4*Mrlu4_5;
+  Mrlu4_12_6 = Mrlu3_12_6-Mrlu12_4*Mrlu4_6;
+  Mrlu4_12_7 = Mrlu3_12_7-Mrlu12_4*Mrlu4_7;
+  Mrlu4_12_8 = Mrlu3_12_8-Mrlu12_4*Mrlu4_8;
+  Mrlu4_12_9 = Mrlu3_12_9-Mrlu12_4*Mrlu4_9;
+  Mrlu4_12_10 = Mrlu3_12_10-Mrlu12_4*Mrlu4_10;
+  Mrlu4_12_11 = Mrlu3_12_11-Mrlu12_4*Mrlu4_11;
+  Mrlu4_12_12 = Mrlu3_12_12-Mrlu12_4*Mrlu4_12;
+  Mrlu6_5 = Mrlu4_6_5/Mrlu5_5;
+  Mrlu6_6 = Mrlu4_6_6-Mrlu5_6*Mrlu6_5;
+  Mrlu6_7 = Mrlu4_6_7-Mrlu5_7*Mrlu6_5;
+  Mrlu6_8 = Mrlu4_6_8-Mrlu5_8*Mrlu6_5;
+  Mrlu6_9 = Mrlu4_6_9-Mrlu5_9*Mrlu6_5;
+  Mrlu6_10 = Mrlu4_6_10-Mrlu5_10*Mrlu6_5;
+  Mrlu6_11 = Mrlu4_6_11-Mrlu5_11*Mrlu6_5;
+  Mrlu6_12 = Mrlu4_6_12-Mrlu5_12*Mrlu6_5;
+  Mrlu7_5 = Mrlu4_7_5/Mrlu5_5;
+  Mrlu5_7_6 = Mrlu4_7_6-Mrlu5_6*Mrlu7_5;
+  Mrlu5_7_7 = Mrlu4_7_7-Mrlu5_7*Mrlu7_5;
+  Mrlu5_7_8 = Mrlu4_7_8-Mrlu5_8*Mrlu7_5;
+  Mrlu5_7_9 = Mrlu4_7_9-Mrlu5_9*Mrlu7_5;
+  Mrlu5_7_10 = Mrlu4_7_10-Mrlu5_10*Mrlu7_5;
+  Mrlu5_7_11 = Mrlu4_7_11-Mrlu5_11*Mrlu7_5;
+  Mrlu5_7_12 = Mrlu4_7_12-Mrlu5_12*Mrlu7_5;
+  Mrlu8_5 = Mrlu4_8_5/Mrlu5_5;
+  Mrlu5_8_6 = Mrlu4_8_6-Mrlu5_6*Mrlu8_5;
+  Mrlu5_8_7 = Mrlu4_8_7-Mrlu5_7*Mrlu8_5;
+  Mrlu5_8_8 = Mrlu4_8_8-Mrlu5_8*Mrlu8_5;
+  Mrlu5_8_9 = Mrlu4_8_9-Mrlu5_9*Mrlu8_5;
+  Mrlu5_8_10 = Mrlu4_8_10-Mrlu5_10*Mrlu8_5;
+  Mrlu5_8_11 = Mrlu4_8_11-Mrlu5_11*Mrlu8_5;
+  Mrlu5_8_12 = Mrlu4_8_12-Mrlu5_12*Mrlu8_5;
+  Mrlu9_5 = Mrlu4_9_5/Mrlu5_5;
+  Mrlu5_9_6 = Mrlu4_9_6-Mrlu5_6*Mrlu9_5;
+  Mrlu5_9_7 = Mrlu4_9_7-Mrlu5_7*Mrlu9_5;
+  Mrlu5_9_8 = Mrlu4_9_8-Mrlu5_8*Mrlu9_5;
+  Mrlu5_9_9 = Mrlu4_9_9-Mrlu5_9*Mrlu9_5;
+  Mrlu5_9_10 = Mrlu4_9_10-Mrlu5_10*Mrlu9_5;
+  Mrlu5_9_11 = Mrlu4_9_11-Mrlu5_11*Mrlu9_5;
+  Mrlu5_9_12 = Mrlu4_9_12-Mrlu5_12*Mrlu9_5;
+  Mrlu10_5 = Mrlu4_10_5/Mrlu5_5;
+  Mrlu5_10_6 = Mrlu4_10_6-Mrlu10_5*Mrlu5_6;
+  Mrlu5_10_7 = Mrlu4_10_7-Mrlu10_5*Mrlu5_7;
+  Mrlu5_10_8 = Mrlu4_10_8-Mrlu10_5*Mrlu5_8;
+  Mrlu5_10_9 = Mrlu4_10_9-Mrlu10_5*Mrlu5_9;
+  Mrlu5_10_10 = Mrlu4_10_10-Mrlu10_5*Mrlu5_10;
+  Mrlu5_10_11 = Mrlu4_10_11-Mrlu10_5*Mrlu5_11;
+  Mrlu5_10_12 = Mrlu4_10_12-Mrlu10_5*Mrlu5_12;
+  Mrlu11_5 = Mrlu4_11_5/Mrlu5_5;
+  Mrlu5_11_6 = Mrlu4_11_6-Mrlu11_5*Mrlu5_6;
+  Mrlu5_11_7 = Mrlu4_11_7-Mrlu11_5*Mrlu5_7;
+  Mrlu5_11_8 = Mrlu4_11_8-Mrlu11_5*Mrlu5_8;
+  Mrlu5_11_9 = Mrlu4_11_9-Mrlu11_5*Mrlu5_9;
+  Mrlu5_11_10 = Mrlu4_11_10-Mrlu11_5*Mrlu5_10;
+  Mrlu5_11_11 = Mrlu4_11_11-Mrlu11_5*Mrlu5_11;
+  Mrlu5_11_12 = Mrlu4_11_12-Mrlu11_5*Mrlu5_12;
+  Mrlu12_5 = Mrlu4_12_5/Mrlu5_5;
+  Mrlu5_12_6 = Mrlu4_12_6-Mrlu12_5*Mrlu5_6;
+  Mrlu5_12_7 = Mrlu4_12_7-Mrlu12_5*Mrlu5_7;
+  Mrlu5_12_8 = Mrlu4_12_8-Mrlu12_5*Mrlu5_8;
+  Mrlu5_12_9 = Mrlu4_12_9-Mrlu12_5*Mrlu5_9;
+  Mrlu5_12_10 = Mrlu4_12_10-Mrlu12_5*Mrlu5_10;
+  Mrlu5_12_11 = Mrlu4_12_11-Mrlu12_5*Mrlu5_11;
+  Mrlu5_12_12 = Mrlu4_12_12-Mrlu12_5*Mrlu5_12;
+  Mrlu7_6 = Mrlu5_7_6/Mrlu6_6;
+  Mrlu7_7 = Mrlu5_7_7-Mrlu6_7*Mrlu7_6;
+  Mrlu7_8 = Mrlu5_7_8-Mrlu6_8*Mrlu7_6;
+  Mrlu7_9 = Mrlu5_7_9-Mrlu6_9*Mrlu7_6;
+  Mrlu7_10 = Mrlu5_7_10-Mrlu6_10*Mrlu7_6;
+  Mrlu7_11 = Mrlu5_7_11-Mrlu6_11*Mrlu7_6;
+  Mrlu7_12 = Mrlu5_7_12-Mrlu6_12*Mrlu7_6;
+  Mrlu8_6 = Mrlu5_8_6/Mrlu6_6;
+  Mrlu6_8_7 = Mrlu5_8_7-Mrlu6_7*Mrlu8_6;
+  Mrlu6_8_8 = Mrlu5_8_8-Mrlu6_8*Mrlu8_6;
+  Mrlu6_8_9 = Mrlu5_8_9-Mrlu6_9*Mrlu8_6;
+  Mrlu6_8_10 = Mrlu5_8_10-Mrlu6_10*Mrlu8_6;
+  Mrlu6_8_11 = Mrlu5_8_11-Mrlu6_11*Mrlu8_6;
+  Mrlu6_8_12 = Mrlu5_8_12-Mrlu6_12*Mrlu8_6;
+  Mrlu9_6 = Mrlu5_9_6/Mrlu6_6;
+  Mrlu6_9_7 = Mrlu5_9_7-Mrlu6_7*Mrlu9_6;
+  Mrlu6_9_8 = Mrlu5_9_8-Mrlu6_8*Mrlu9_6;
+  Mrlu6_9_9 = Mrlu5_9_9-Mrlu6_9*Mrlu9_6;
+  Mrlu6_9_10 = Mrlu5_9_10-Mrlu6_10*Mrlu9_6;
+  Mrlu6_9_11 = Mrlu5_9_11-Mrlu6_11*Mrlu9_6;
+  Mrlu6_9_12 = Mrlu5_9_12-Mrlu6_12*Mrlu9_6;
+  Mrlu10_6 = Mrlu5_10_6/Mrlu6_6;
+  Mrlu6_10_7 = Mrlu5_10_7-Mrlu10_6*Mrlu6_7;
+  Mrlu6_10_8 = Mrlu5_10_8-Mrlu10_6*Mrlu6_8;
+  Mrlu6_10_9 = Mrlu5_10_9-Mrlu10_6*Mrlu6_9;
+  Mrlu6_10_10 = Mrlu5_10_10-Mrlu10_6*Mrlu6_10;
+  Mrlu6_10_11 = Mrlu5_10_11-Mrlu10_6*Mrlu6_11;
+  Mrlu6_10_12 = Mrlu5_10_12-Mrlu10_6*Mrlu6_12;
+  Mrlu11_6 = Mrlu5_11_6/Mrlu6_6;
+  Mrlu6_11_7 = Mrlu5_11_7-Mrlu11_6*Mrlu6_7;
+  Mrlu6_11_8 = Mrlu5_11_8-Mrlu11_6*Mrlu6_8;
+  Mrlu6_11_9 = Mrlu5_11_9-Mrlu11_6*Mrlu6_9;
+  Mrlu6_11_10 = Mrlu5_11_10-Mrlu11_6*Mrlu6_10;
+  Mrlu6_11_11 = Mrlu5_11_11-Mrlu11_6*Mrlu6_11;
+  Mrlu6_11_12 = Mrlu5_11_12-Mrlu11_6*Mrlu6_12;
+  Mrlu12_6 = Mrlu5_12_6/Mrlu6_6;
+  Mrlu6_12_7 = Mrlu5_12_7-Mrlu12_6*Mrlu6_7;
+  Mrlu6_12_8 = Mrlu5_12_8-Mrlu12_6*Mrlu6_8;
+  Mrlu6_12_9 = Mrlu5_12_9-Mrlu12_6*Mrlu6_9;
+  Mrlu6_12_10 = Mrlu5_12_10-Mrlu12_6*Mrlu6_10;
+  Mrlu6_12_11 = Mrlu5_12_11-Mrlu12_6*Mrlu6_11;
+  Mrlu6_12_12 = Mrlu5_12_12-Mrlu12_6*Mrlu6_12;
+  Mrlu8_7 = Mrlu6_8_7/Mrlu7_7;
+  Mrlu8_8 = Mrlu6_8_8-Mrlu7_8*Mrlu8_7;
+  Mrlu8_9 = Mrlu6_8_9-Mrlu7_9*Mrlu8_7;
+  Mrlu8_10 = Mrlu6_8_10-Mrlu7_10*Mrlu8_7;
+  Mrlu8_11 = Mrlu6_8_11-Mrlu7_11*Mrlu8_7;
+  Mrlu8_12 = Mrlu6_8_12-Mrlu7_12*Mrlu8_7;
+  Mrlu9_7 = Mrlu6_9_7/Mrlu7_7;
+  Mrlu7_9_8 = Mrlu6_9_8-Mrlu7_8*Mrlu9_7;
+  Mrlu7_9_9 = Mrlu6_9_9-Mrlu7_9*Mrlu9_7;
+  Mrlu7_9_10 = Mrlu6_9_10-Mrlu7_10*Mrlu9_7;
+  Mrlu7_9_11 = Mrlu6_9_11-Mrlu7_11*Mrlu9_7;
+  Mrlu7_9_12 = Mrlu6_9_12-Mrlu7_12*Mrlu9_7;
+  Mrlu10_7 = Mrlu6_10_7/Mrlu7_7;
+  Mrlu7_10_8 = Mrlu6_10_8-Mrlu10_7*Mrlu7_8;
+  Mrlu7_10_9 = Mrlu6_10_9-Mrlu10_7*Mrlu7_9;
+  Mrlu7_10_10 = Mrlu6_10_10-Mrlu10_7*Mrlu7_10;
+  Mrlu7_10_11 = Mrlu6_10_11-Mrlu10_7*Mrlu7_11;
+  Mrlu7_10_12 = Mrlu6_10_12-Mrlu10_7*Mrlu7_12;
+  Mrlu11_7 = Mrlu6_11_7/Mrlu7_7;
+  Mrlu7_11_8 = Mrlu6_11_8-Mrlu11_7*Mrlu7_8;
+  Mrlu7_11_9 = Mrlu6_11_9-Mrlu11_7*Mrlu7_9;
+  Mrlu7_11_10 = Mrlu6_11_10-Mrlu11_7*Mrlu7_10;
+  Mrlu7_11_11 = Mrlu6_11_11-Mrlu11_7*Mrlu7_11;
+  Mrlu7_11_12 = Mrlu6_11_12-Mrlu11_7*Mrlu7_12;
+  Mrlu12_7 = Mrlu6_12_7/Mrlu7_7;
+  Mrlu7_12_8 = Mrlu6_12_8-Mrlu12_7*Mrlu7_8;
+  Mrlu7_12_9 = Mrlu6_12_9-Mrlu12_7*Mrlu7_9;
+  Mrlu7_12_10 = Mrlu6_12_10-Mrlu12_7*Mrlu7_10;
+  Mrlu7_12_11 = Mrlu6_12_11-Mrlu12_7*Mrlu7_11;
+  Mrlu7_12_12 = Mrlu6_12_12-Mrlu12_7*Mrlu7_12;
+  Mrlu9_8 = Mrlu7_9_8/Mrlu8_8;
+  Mrlu9_9 = Mrlu7_9_9-Mrlu8_9*Mrlu9_8;
+  Mrlu9_10 = Mrlu7_9_10-Mrlu8_10*Mrlu9_8;
+  Mrlu9_11 = Mrlu7_9_11-Mrlu8_11*Mrlu9_8;
+  Mrlu9_12 = Mrlu7_9_12-Mrlu8_12*Mrlu9_8;
+  Mrlu10_8 = Mrlu7_10_8/Mrlu8_8;
+  Mrlu8_10_9 = Mrlu7_10_9-Mrlu10_8*Mrlu8_9;
+  Mrlu8_10_10 = Mrlu7_10_10-Mrlu10_8*Mrlu8_10;
+  Mrlu8_10_11 = Mrlu7_10_11-Mrlu10_8*Mrlu8_11;
+  Mrlu8_10_12 = Mrlu7_10_12-Mrlu10_8*Mrlu8_12;
+  Mrlu11_8 = Mrlu7_11_8/Mrlu8_8;
+  Mrlu8_11_9 = Mrlu7_11_9-Mrlu11_8*Mrlu8_9;
+  Mrlu8_11_10 = Mrlu7_11_10-Mrlu11_8*Mrlu8_10;
+  Mrlu8_11_11 = Mrlu7_11_11-Mrlu11_8*Mrlu8_11;
+  Mrlu8_11_12 = Mrlu7_11_12-Mrlu11_8*Mrlu8_12;
+  Mrlu12_8 = Mrlu7_12_8/Mrlu8_8;
+  Mrlu8_12_9 = Mrlu7_12_9-Mrlu12_8*Mrlu8_9;
+  Mrlu8_12_10 = Mrlu7_12_10-Mrlu12_8*Mrlu8_10;
+  Mrlu8_12_11 = Mrlu7_12_11-Mrlu12_8*Mrlu8_11;
+  Mrlu8_12_12 = Mrlu7_12_12-Mrlu12_8*Mrlu8_12;
+  Mrlu10_9 = Mrlu8_10_9/Mrlu9_9;
+  Mrlu10_10 = Mrlu8_10_10-Mrlu10_9*Mrlu9_10;
+  Mrlu10_11 = Mrlu8_10_11-Mrlu10_9*Mrlu9_11;
+  Mrlu10_12 = Mrlu8_10_12-Mrlu10_9*Mrlu9_12;
+  Mrlu11_9 = Mrlu8_11_9/Mrlu9_9;
+  Mrlu9_11_10 = Mrlu8_11_10-Mrlu11_9*Mrlu9_10;
+  Mrlu9_11_11 = Mrlu8_11_11-Mrlu11_9*Mrlu9_11;
+  Mrlu9_11_12 = Mrlu8_11_12-Mrlu11_9*Mrlu9_12;
+  Mrlu12_9 = Mrlu8_12_9/Mrlu9_9;
+  Mrlu9_12_10 = Mrlu8_12_10-Mrlu12_9*Mrlu9_10;
+  Mrlu9_12_11 = Mrlu8_12_11-Mrlu12_9*Mrlu9_11;
+  Mrlu9_12_12 = Mrlu8_12_12-Mrlu12_9*Mrlu9_12;
+  Mrlu11_10 = Mrlu9_11_10/Mrlu10_10;
+  Mrlu11_11 = Mrlu9_11_11-Mrlu10_11*Mrlu11_10;
+  Mrlu11_12 = Mrlu9_11_12-Mrlu10_12*Mrlu11_10;
+  Mrlu12_10 = Mrlu9_12_10/Mrlu10_10;
+  Mrlu10_12_11 = Mrlu9_12_11-Mrlu10_11*Mrlu12_10;
+  Mrlu10_12_12 = Mrlu9_12_12-Mrlu10_12*Mrlu12_10;
+  Mrlu12_11 = Mrlu10_12_11/Mrlu11_11;
+  Mrlu12_12 = Mrlu10_12_12-Mrlu11_12*Mrlu12_11;
+  qppuy2 = -Fr2+Fr1*Mrlu2_1;
+  qppuy3 = -Fr3+Fr1*Mrlu3_1-Mrlu3_2*qppuy2;
+  qppuy4 = -Fr4+Fr1*Mrlu4_1-Mrlu4_2*qppuy2-Mrlu4_3*qppuy3;
+  qppuy5 = -Fr5+Fr1*Mrlu5_1-Mrlu5_2*qppuy2-Mrlu5_3*qppuy3-Mrlu5_4*qppuy4;
+  qppuy6 = -Fr6+Fr1*Mrlu6_1-Mrlu6_2*qppuy2-Mrlu6_3*qppuy3-Mrlu6_4*qppuy4-Mrlu6_5*qppuy5;
+  qppuy7 = -Fr7+Fr1*Mrlu7_1-Mrlu7_2*qppuy2-Mrlu7_3*qppuy3-Mrlu7_4*qppuy4-Mrlu7_5*qppuy5-Mrlu7_6*qppuy6;
+  qppuy8 = -Fr8+Fr1*Mrlu8_1-Mrlu8_2*qppuy2-Mrlu8_3*qppuy3-Mrlu8_4*qppuy4-Mrlu8_5*qppuy5-Mrlu8_6*qppuy6-Mrlu8_7*qppuy7;
+  qppuy9 = -Fr9+Fr1*Mrlu9_1-Mrlu9_2*qppuy2-Mrlu9_3*qppuy3-Mrlu9_4*qppuy4-Mrlu9_5*qppuy5-Mrlu9_6*qppuy6-Mrlu9_7*qppuy7-
+ Mrlu9_8*qppuy8;
+  qppuy10 = -Fr10+Fr1*Mrlu10_1-Mrlu10_2*qppuy2-Mrlu10_3*qppuy3-Mrlu10_4*qppuy4-Mrlu10_5*qppuy5-Mrlu10_6*qppuy6-Mrlu10_7*
+ qppuy7-Mrlu10_8*qppuy8-Mrlu10_9*qppuy9;
+  qppuy11 = -Fr11+Fr1*Mrlu11_1-Mrlu11_2*qppuy2-Mrlu11_3*qppuy3-Mrlu11_4*qppuy4-Mrlu11_5*qppuy5-Mrlu11_6*qppuy6-Mrlu11_7*
+ qppuy7-Mrlu11_8*qppuy8-Mrlu11_9*qppuy9-Mrlu11_10*qppuy10;
+  qppuy12 = -Fr12+Fr1*Mrlu12_1-Mrlu12_2*qppuy2-Mrlu12_3*qppuy3-Mrlu12_4*qppuy4-Mrlu12_5*qppuy5-Mrlu12_6*qppuy6-Mrlu12_7*
+ qppuy7-Mrlu12_8*qppuy8-Mrlu12_9*qppuy9-Mrlu12_10*qppuy10-Mrlu12_11*qppuy11;
+  qppux12 = qppuy12/Mrlu12_12;
+  qppux11 = (qppuy11-Mrlu11_12*qppux12)/Mrlu11_11;
+  qppux10 = (qppuy10-Mrlu10_12*qppux12-Mrlu10_11*qppux11)/Mrlu10_10;
+  qppux9 = (qppuy9-Mrlu9_12*qppux12-Mrlu9_11*qppux11-Mrlu9_10*qppux10)/Mrlu9_9;
+  qppux8 = (qppuy8-Mrlu8_12*qppux12-Mrlu8_11*qppux11-Mrlu8_10*qppux10-Mrlu8_9*qppux9)/Mrlu8_8;
+  qppux7 = (qppuy7-Mrlu7_12*qppux12-Mrlu7_11*qppux11-Mrlu7_10*qppux10-Mrlu7_9*qppux9-Mrlu7_8*qppux8)/Mrlu7_7;
+  qppux6 = (qppuy6-Mrlu6_12*qppux12-Mrlu6_11*qppux11-Mrlu6_10*qppux10-Mrlu6_9*qppux9-Mrlu6_8*qppux8-Mrlu6_7*qppux7)/
+ Mrlu6_6;
+  qppux5 = (qppuy5-Mrlu5_12*qppux12-Mrlu5_11*qppux11-Mrlu5_10*qppux10-Mrlu5_9*qppux9-Mrlu5_8*qppux8-Mrlu5_7*qppux7-
+ Mrlu5_6*qppux6)/Mrlu5_5;
+  qppux4 = (qppuy4-Mrlu4_12*qppux12-Mrlu4_11*qppux11-Mrlu4_10*qppux10-Mrlu4_9*qppux9-Mrlu4_8*qppux8-Mrlu4_7*qppux7-
+ Mrlu4_6*qppux6-Mrlu4_5*qppux5)/Mrlu4_4;
+  qppux3 = (qppuy3-Mrlu3_12*qppux12-Mrlu3_11*qppux11-Mrlu3_10*qppux10-Mrlu3_9*qppux9-Mrlu3_8*qppux8-Mrlu3_7*qppux7-
+ Mrlu3_6*qppux6-Mrlu3_5*qppux5-Mrlu3_4*qppux4)/Mrlu3_3;
+  qppux2 = (qppuy2-Mrlu2_12*qppux12-Mrlu2_11*qppux11-Mrlu2_10*qppux10-Mrlu2_9*qppux9-Mrlu2_8*qppux8-Mrlu2_7*qppux7-
+ Mrlu2_6*qppux6-Mrlu2_5*qppux5-Mrlu2_4*qppux4-Mrlu2_3*qppux3)/Mrlu2_2;
+  qppux1 = (-Fr1-CM212_1*qppux12-CM211_1*qppux11-CM210_1*qppux10-CM29_1*qppux9-CM28_1*qppux8-CM27_1*qppux7-CM26_1*qppux6
+ -CM25_1*qppux5-CM24_1*qppux4-CM23_1*qppux3-FM32_1*qppux2)/FM12_1;
+ 
+// Symbolic Output number 1
+
+  qdd[1] = qppux1;
+  qdd[2] = qppux2;
+  qdd[3] = qppux3;
+  qdd[4] = qppux4;
+  qdd[5] = qppux5;
+  qdd[6] = qppux6;
+  qdd[7] = qppux7;
+  qdd[8] = qppux8;
+  qdd[9] = qppux9;
+  qdd[10] = qppux10;
+  qdd[11] = qppux11;
+  qdd[12] = qppux12;
+ 
+return iter; 
+
+
+// Number of continuation lines = 1
+
+}
diff --git a/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.h b/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.h
new file mode 100644
index 0000000..fdfc1d9
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_accelred_coman_short_feet_simp.h
@@ -0,0 +1,1068 @@
+double    S3;
+double    C3;
+double    S4;
+double    C4;
+double    S5;
+double    C5;
+double    S6;
+double    C6;
+double    S7;
+double    C7;
+double    S8;
+double    C8;
+double    S9;
+double    C9;
+double    S10;
+double    C10;
+double    S11;
+double    C11;
+double    S12;
+double    C12;
+double    ROcp1_14;
+double    ROcp1_34;
+double    ROcp1_74;
+double    ROcp1_94;
+double    ROcp1_15;
+double    ROcp1_35;
+double    ROcp1_75;
+double    ROcp1_95;
+double    ROcp1_16;
+double    ROcp1_36;
+double    ROcp1_76;
+double    ROcp1_96;
+double    OMcp1_24;
+double    RLcp1_15;
+double    RLcp1_35;
+double    POcp1_15;
+double    POcp1_25;
+double    POcp1_35;
+double    OMcp1_25;
+double    ORcp1_15;
+double    ORcp1_35;
+double    VIcp1_15;
+double    VIcp1_35;
+double    RLcp1_16;
+double    RLcp1_36;
+double    POcp1_16;
+double    POcp1_36;
+double    OMcp1_26;
+double    ORcp1_16;
+double    ORcp1_36;
+double    VIcp1_16;
+double    VIcp1_36;
+double    RLcp1_17;
+double    RLcp1_37;
+double    POcp1_17;
+double    POcp1_37;
+double    ORcp1_17;
+double    ORcp1_37;
+double    VIcp1_17;
+double    VIcp1_37;
+double    xfrc11;
+double    xfrc21;
+double    xfrc31;
+double    frcext_1_6;
+double    frcext_2_6;
+double    frcext_3_6;
+double    xtrq11;
+double    xtrq21;
+double    xtrq31;
+double    trqext_1_6;
+double    trqext_2_6;
+double    trqext_3_6;
+double    ROcp2_17;
+double    ROcp2_37;
+double    ROcp2_77;
+double    ROcp2_97;
+double    ROcp2_18;
+double    ROcp2_38;
+double    ROcp2_78;
+double    ROcp2_98;
+double    ROcp2_19;
+double    ROcp2_39;
+double    ROcp2_79;
+double    ROcp2_99;
+double    OMcp2_24;
+double    RLcp2_15;
+double    RLcp2_35;
+double    POcp2_15;
+double    POcp2_25;
+double    POcp2_35;
+double    OMcp2_25;
+double    ORcp2_15;
+double    ORcp2_35;
+double    VIcp2_15;
+double    VIcp2_35;
+double    RLcp2_16;
+double    RLcp2_36;
+double    POcp2_16;
+double    POcp2_36;
+double    OMcp2_26;
+double    ORcp2_16;
+double    ORcp2_36;
+double    VIcp2_16;
+double    VIcp2_36;
+double    RLcp2_17;
+double    RLcp2_37;
+double    POcp2_17;
+double    POcp2_37;
+double    ORcp2_17;
+double    ORcp2_37;
+double    VIcp2_17;
+double    VIcp2_37;
+double    xfrc12;
+double    xfrc22;
+double    xfrc32;
+double    frcext_1_9;
+double    frcext_2_9;
+double    frcext_3_9;
+double    xtrq12;
+double    xtrq22;
+double    xtrq32;
+double    trqext_1_9;
+double    trqext_2_9;
+double    trqext_3_9;
+double    BS13;
+double    BS93;
+double    AlF13;
+double    AlF33;
+double    OM24;
+double    BS14;
+double    BS94;
+double    AlF14;
+double    AlF34;
+double    AlM14_1;
+double    AlM34_1;
+double    AlM14_2;
+double    AlM34_2;
+double    OM25;
+double    BS15;
+double    BS95;
+double    AlF15;
+double    AlF35;
+double    AlM15_1;
+double    AlM35_1;
+double    AlM15_2;
+double    AlM35_2;
+double    AlM15_3;
+double    AlM35_3;
+double    AlM15_4;
+double    AlM35_4;
+double    OM26;
+double    BS16;
+double    BS96;
+double    AlF16;
+double    AlF36;
+double    AlM16_1;
+double    AlM36_1;
+double    AlM16_2;
+double    AlM36_2;
+double    AlM16_3;
+double    AlM36_3;
+double    AlM16_4;
+double    AlM36_4;
+double    AlM16_5;
+double    AlM36_5;
+double    OM27;
+double    BS17;
+double    BS97;
+double    AlF17;
+double    AlF37;
+double    AlM17_1;
+double    AlM37_1;
+double    AlM17_2;
+double    AlM37_2;
+double    OM28;
+double    BS18;
+double    BS98;
+double    AlF18;
+double    AlF38;
+double    AlM18_1;
+double    AlM38_1;
+double    AlM18_2;
+double    AlM38_2;
+double    AlM18_3;
+double    AlM38_3;
+double    AlM18_7;
+double    AlM38_7;
+double    OM29;
+double    BS19;
+double    BS99;
+double    AlF19;
+double    AlF39;
+double    AlM19_1;
+double    AlM39_1;
+double    AlM19_2;
+double    AlM39_2;
+double    AlM19_3;
+double    AlM39_3;
+double    AlM19_7;
+double    AlM39_7;
+double    AlM19_8;
+double    AlM39_8;
+double    OM210;
+double    BS110;
+double    BS910;
+double    AlF110;
+double    AlF310;
+double    AlM110_1;
+double    AlM310_1;
+double    AlM110_2;
+double    AlM310_2;
+double    AlM110_3;
+double    AlM310_3;
+double    OM211;
+double    BS111;
+double    BS911;
+double    AlF111;
+double    AlF311;
+double    AlM111_1;
+double    AlM311_1;
+double    AlM111_2;
+double    AlM311_2;
+double    AlM111_3;
+double    AlM311_3;
+double    AlM111_10;
+double    AlM311_10;
+double    OM212;
+double    BS112;
+double    BS912;
+double    AlF112;
+double    AlF312;
+double    AlM112_1;
+double    AlM312_1;
+double    AlM112_2;
+double    AlM312_2;
+double    AlM112_3;
+double    AlM312_3;
+double    AlM112_10;
+double    AlM312_10;
+double    FA112;
+double    FA312;
+double    CF212;
+double    FB112_1;
+double    FB312_1;
+double    CM212_1;
+double    FB112_2;
+double    FB312_2;
+double    CM212_2;
+double    FB112_3;
+double    FB312_3;
+double    CM212_3;
+double    FB112_10;
+double    FB312_10;
+double    CM212_10;
+double    FB112_12;
+double    FB312_12;
+double    CM212_12;
+double    FA111;
+double    FA311;
+double    CF211;
+double    FB111_1;
+double    FB311_1;
+double    CM211_1;
+double    FB111_2;
+double    FB311_2;
+double    CM211_2;
+double    FB111_3;
+double    FB311_3;
+double    CM211_3;
+double    FB111_10;
+double    FB311_10;
+double    CM211_10;
+double    FB111_11;
+double    FB311_11;
+double    CM211_11;
+double    FA110;
+double    FA310;
+double    FF110;
+double    FF310;
+double    CF210;
+double    FB110_1;
+double    FB310_1;
+double    FM110_1;
+double    FM310_1;
+double    CM210_1;
+double    FB110_2;
+double    FB310_2;
+double    FM110_2;
+double    FM310_2;
+double    CM210_2;
+double    FB110_3;
+double    FB310_3;
+double    FM110_3;
+double    FM310_3;
+double    CM210_3;
+double    FB110_10;
+double    FB310_10;
+double    CM210_10;
+double    FA19;
+double    FA39;
+double    CF29;
+double    FB19_1;
+double    FB39_1;
+double    CM29_1;
+double    FB19_2;
+double    FB39_2;
+double    CM29_2;
+double    FB19_3;
+double    FB39_3;
+double    CM29_3;
+double    FB19_7;
+double    FB39_7;
+double    CM29_7;
+double    FB19_8;
+double    FB39_8;
+double    CM29_8;
+double    FB19_9;
+double    FB39_9;
+double    CM29_9;
+double    FA18;
+double    FA38;
+double    FF18;
+double    FF38;
+double    CF28;
+double    FB18_1;
+double    FB38_1;
+double    FM18_1;
+double    FM38_1;
+double    CM28_1;
+double    FB18_2;
+double    FB38_2;
+double    FM18_2;
+double    FM38_2;
+double    CM28_2;
+double    FB18_3;
+double    FB38_3;
+double    FM18_3;
+double    FM38_3;
+double    CM28_3;
+double    FB18_7;
+double    FB38_7;
+double    FM18_7;
+double    FM38_7;
+double    CM28_7;
+double    FB18_8;
+double    FB38_8;
+double    CM28_8;
+double    FA17;
+double    FA37;
+double    FF17;
+double    FF37;
+double    CF27;
+double    FB17_1;
+double    FB37_1;
+double    FM17_1;
+double    FM37_1;
+double    CM27_1;
+double    FB17_2;
+double    FB37_2;
+double    FM17_2;
+double    FM37_2;
+double    CM27_2;
+double    FB17_3;
+double    FB37_3;
+double    CM27_3;
+double    FB17_7;
+double    FB37_7;
+double    CM27_7;
+double    FA16;
+double    FA36;
+double    CF26;
+double    FB16_1;
+double    FB36_1;
+double    CM26_1;
+double    FB16_2;
+double    FB36_2;
+double    CM26_2;
+double    FB16_3;
+double    FB36_3;
+double    CM26_3;
+double    FB16_4;
+double    FB36_4;
+double    CM26_4;
+double    FB16_5;
+double    FB36_5;
+double    CM26_5;
+double    FB16_6;
+double    FB36_6;
+double    CM26_6;
+double    FA15;
+double    FA35;
+double    FF15;
+double    FF35;
+double    CF25;
+double    FB15_1;
+double    FB35_1;
+double    FM15_1;
+double    FM35_1;
+double    CM25_1;
+double    FB15_2;
+double    FB35_2;
+double    FM15_2;
+double    FM35_2;
+double    CM25_2;
+double    FB15_3;
+double    FB35_3;
+double    FM15_3;
+double    FM35_3;
+double    CM25_3;
+double    FB15_4;
+double    FB35_4;
+double    FM15_4;
+double    FM35_4;
+double    CM25_4;
+double    FB15_5;
+double    FB35_5;
+double    CM25_5;
+double    FA14;
+double    FA34;
+double    FF14;
+double    FF34;
+double    CF24;
+double    FB14_1;
+double    FB34_1;
+double    FM14_1;
+double    FM34_1;
+double    CM24_1;
+double    FB14_2;
+double    FB34_2;
+double    FM14_2;
+double    FM34_2;
+double    CM24_2;
+double    FB14_3;
+double    FB34_3;
+double    CM24_3;
+double    FB14_4;
+double    FB34_4;
+double    CM24_4;
+double    FA13;
+double    FA33;
+double    FF13;
+double    FF33;
+double    CF23;
+double    FB13_1;
+double    FB33_1;
+double    FM13_1;
+double    FM33_1;
+double    CM23_1;
+double    FB13_2;
+double    FB33_2;
+double    FM13_2;
+double    FM33_2;
+double    CM23_2;
+double    FB13_3;
+double    FB33_3;
+double    CM23_3;
+double    FF12;
+double    FF32;
+double    FM12_1;
+double    FM32_1;
+double    FM32_2;
+double    Fr1;
+double    Fr2;
+double    Fr3;
+double    Fr4;
+double    Fr5;
+double    Fr6;
+double    Fr7;
+double    Fr8;
+double    Fr9;
+double    Fr10;
+double    Fr11;
+double    Fr12;
+double    Mrlu2_1;
+double    Mrlu2_2;
+double    Mrlu2_3;
+double    Mrlu2_4;
+double    Mrlu2_5;
+double    Mrlu2_6;
+double    Mrlu2_7;
+double    Mrlu2_8;
+double    Mrlu2_9;
+double    Mrlu2_10;
+double    Mrlu2_11;
+double    Mrlu2_12;
+double    Mrlu3_1;
+double    Mrlu1_3_2;
+double    Mrlu1_3_3;
+double    Mrlu1_3_4;
+double    Mrlu1_3_5;
+double    Mrlu1_3_6;
+double    Mrlu1_3_7;
+double    Mrlu1_3_8;
+double    Mrlu1_3_9;
+double    Mrlu1_3_10;
+double    Mrlu1_3_11;
+double    Mrlu1_3_12;
+double    Mrlu4_1;
+double    Mrlu1_4_2;
+double    Mrlu1_4_3;
+double    Mrlu1_4_4;
+double    Mrlu1_4_5;
+double    Mrlu1_4_6;
+double    Mrlu1_4_7;
+double    Mrlu1_4_8;
+double    Mrlu1_4_9;
+double    Mrlu1_4_10;
+double    Mrlu1_4_11;
+double    Mrlu1_4_12;
+double    Mrlu5_1;
+double    Mrlu1_5_2;
+double    Mrlu1_5_3;
+double    Mrlu1_5_4;
+double    Mrlu1_5_5;
+double    Mrlu1_5_6;
+double    Mrlu1_5_7;
+double    Mrlu1_5_8;
+double    Mrlu1_5_9;
+double    Mrlu1_5_10;
+double    Mrlu1_5_11;
+double    Mrlu1_5_12;
+double    Mrlu6_1;
+double    Mrlu1_6_2;
+double    Mrlu1_6_3;
+double    Mrlu1_6_4;
+double    Mrlu1_6_5;
+double    Mrlu1_6_6;
+double    Mrlu1_6_7;
+double    Mrlu1_6_8;
+double    Mrlu1_6_9;
+double    Mrlu1_6_10;
+double    Mrlu1_6_11;
+double    Mrlu1_6_12;
+double    Mrlu7_1;
+double    Mrlu1_7_2;
+double    Mrlu1_7_3;
+double    Mrlu1_7_4;
+double    Mrlu1_7_5;
+double    Mrlu1_7_6;
+double    Mrlu1_7_7;
+double    Mrlu1_7_8;
+double    Mrlu1_7_9;
+double    Mrlu1_7_10;
+double    Mrlu1_7_11;
+double    Mrlu1_7_12;
+double    Mrlu8_1;
+double    Mrlu1_8_2;
+double    Mrlu1_8_3;
+double    Mrlu1_8_4;
+double    Mrlu1_8_5;
+double    Mrlu1_8_6;
+double    Mrlu1_8_7;
+double    Mrlu1_8_8;
+double    Mrlu1_8_9;
+double    Mrlu1_8_10;
+double    Mrlu1_8_11;
+double    Mrlu1_8_12;
+double    Mrlu9_1;
+double    Mrlu1_9_2;
+double    Mrlu1_9_3;
+double    Mrlu1_9_4;
+double    Mrlu1_9_5;
+double    Mrlu1_9_6;
+double    Mrlu1_9_7;
+double    Mrlu1_9_8;
+double    Mrlu1_9_9;
+double    Mrlu1_9_10;
+double    Mrlu1_9_11;
+double    Mrlu1_9_12;
+double    Mrlu10_1;
+double    Mrlu1_10_2;
+double    Mrlu1_10_3;
+double    Mrlu1_10_4;
+double    Mrlu1_10_5;
+double    Mrlu1_10_6;
+double    Mrlu1_10_7;
+double    Mrlu1_10_8;
+double    Mrlu1_10_9;
+double    Mrlu1_10_10;
+double    Mrlu1_10_11;
+double    Mrlu1_10_12;
+double    Mrlu11_1;
+double    Mrlu1_11_2;
+double    Mrlu1_11_3;
+double    Mrlu1_11_4;
+double    Mrlu1_11_5;
+double    Mrlu1_11_6;
+double    Mrlu1_11_7;
+double    Mrlu1_11_8;
+double    Mrlu1_11_9;
+double    Mrlu1_11_10;
+double    Mrlu1_11_11;
+double    Mrlu1_11_12;
+double    Mrlu12_1;
+double    Mrlu1_12_2;
+double    Mrlu1_12_3;
+double    Mrlu1_12_4;
+double    Mrlu1_12_5;
+double    Mrlu1_12_6;
+double    Mrlu1_12_7;
+double    Mrlu1_12_8;
+double    Mrlu1_12_9;
+double    Mrlu1_12_10;
+double    Mrlu1_12_11;
+double    Mrlu1_12_12;
+double    Mrlu3_2;
+double    Mrlu3_3;
+double    Mrlu3_4;
+double    Mrlu3_5;
+double    Mrlu3_6;
+double    Mrlu3_7;
+double    Mrlu3_8;
+double    Mrlu3_9;
+double    Mrlu3_10;
+double    Mrlu3_11;
+double    Mrlu3_12;
+double    Mrlu4_2;
+double    Mrlu2_4_3;
+double    Mrlu2_4_4;
+double    Mrlu2_4_5;
+double    Mrlu2_4_6;
+double    Mrlu2_4_7;
+double    Mrlu2_4_8;
+double    Mrlu2_4_9;
+double    Mrlu2_4_10;
+double    Mrlu2_4_11;
+double    Mrlu2_4_12;
+double    Mrlu5_2;
+double    Mrlu2_5_3;
+double    Mrlu2_5_4;
+double    Mrlu2_5_5;
+double    Mrlu2_5_6;
+double    Mrlu2_5_7;
+double    Mrlu2_5_8;
+double    Mrlu2_5_9;
+double    Mrlu2_5_10;
+double    Mrlu2_5_11;
+double    Mrlu2_5_12;
+double    Mrlu6_2;
+double    Mrlu2_6_3;
+double    Mrlu2_6_4;
+double    Mrlu2_6_5;
+double    Mrlu2_6_6;
+double    Mrlu2_6_7;
+double    Mrlu2_6_8;
+double    Mrlu2_6_9;
+double    Mrlu2_6_10;
+double    Mrlu2_6_11;
+double    Mrlu2_6_12;
+double    Mrlu7_2;
+double    Mrlu2_7_3;
+double    Mrlu2_7_4;
+double    Mrlu2_7_5;
+double    Mrlu2_7_6;
+double    Mrlu2_7_7;
+double    Mrlu2_7_8;
+double    Mrlu2_7_9;
+double    Mrlu2_7_10;
+double    Mrlu2_7_11;
+double    Mrlu2_7_12;
+double    Mrlu8_2;
+double    Mrlu2_8_3;
+double    Mrlu2_8_4;
+double    Mrlu2_8_5;
+double    Mrlu2_8_6;
+double    Mrlu2_8_7;
+double    Mrlu2_8_8;
+double    Mrlu2_8_9;
+double    Mrlu2_8_10;
+double    Mrlu2_8_11;
+double    Mrlu2_8_12;
+double    Mrlu9_2;
+double    Mrlu2_9_3;
+double    Mrlu2_9_4;
+double    Mrlu2_9_5;
+double    Mrlu2_9_6;
+double    Mrlu2_9_7;
+double    Mrlu2_9_8;
+double    Mrlu2_9_9;
+double    Mrlu2_9_10;
+double    Mrlu2_9_11;
+double    Mrlu2_9_12;
+double    Mrlu10_2;
+double    Mrlu2_10_3;
+double    Mrlu2_10_4;
+double    Mrlu2_10_5;
+double    Mrlu2_10_6;
+double    Mrlu2_10_7;
+double    Mrlu2_10_8;
+double    Mrlu2_10_9;
+double    Mrlu2_10_10;
+double    Mrlu2_10_11;
+double    Mrlu2_10_12;
+double    Mrlu11_2;
+double    Mrlu2_11_3;
+double    Mrlu2_11_4;
+double    Mrlu2_11_5;
+double    Mrlu2_11_6;
+double    Mrlu2_11_7;
+double    Mrlu2_11_8;
+double    Mrlu2_11_9;
+double    Mrlu2_11_10;
+double    Mrlu2_11_11;
+double    Mrlu2_11_12;
+double    Mrlu12_2;
+double    Mrlu2_12_3;
+double    Mrlu2_12_4;
+double    Mrlu2_12_5;
+double    Mrlu2_12_6;
+double    Mrlu2_12_7;
+double    Mrlu2_12_8;
+double    Mrlu2_12_9;
+double    Mrlu2_12_10;
+double    Mrlu2_12_11;
+double    Mrlu2_12_12;
+double    Mrlu4_3;
+double    Mrlu4_4;
+double    Mrlu4_5;
+double    Mrlu4_6;
+double    Mrlu4_7;
+double    Mrlu4_8;
+double    Mrlu4_9;
+double    Mrlu4_10;
+double    Mrlu4_11;
+double    Mrlu4_12;
+double    Mrlu5_3;
+double    Mrlu3_5_4;
+double    Mrlu3_5_5;
+double    Mrlu3_5_6;
+double    Mrlu3_5_7;
+double    Mrlu3_5_8;
+double    Mrlu3_5_9;
+double    Mrlu3_5_10;
+double    Mrlu3_5_11;
+double    Mrlu3_5_12;
+double    Mrlu6_3;
+double    Mrlu3_6_4;
+double    Mrlu3_6_5;
+double    Mrlu3_6_6;
+double    Mrlu3_6_7;
+double    Mrlu3_6_8;
+double    Mrlu3_6_9;
+double    Mrlu3_6_10;
+double    Mrlu3_6_11;
+double    Mrlu3_6_12;
+double    Mrlu7_3;
+double    Mrlu3_7_4;
+double    Mrlu3_7_5;
+double    Mrlu3_7_6;
+double    Mrlu3_7_7;
+double    Mrlu3_7_8;
+double    Mrlu3_7_9;
+double    Mrlu3_7_10;
+double    Mrlu3_7_11;
+double    Mrlu3_7_12;
+double    Mrlu8_3;
+double    Mrlu3_8_4;
+double    Mrlu3_8_5;
+double    Mrlu3_8_6;
+double    Mrlu3_8_7;
+double    Mrlu3_8_8;
+double    Mrlu3_8_9;
+double    Mrlu3_8_10;
+double    Mrlu3_8_11;
+double    Mrlu3_8_12;
+double    Mrlu9_3;
+double    Mrlu3_9_4;
+double    Mrlu3_9_5;
+double    Mrlu3_9_6;
+double    Mrlu3_9_7;
+double    Mrlu3_9_8;
+double    Mrlu3_9_9;
+double    Mrlu3_9_10;
+double    Mrlu3_9_11;
+double    Mrlu3_9_12;
+double    Mrlu10_3;
+double    Mrlu3_10_4;
+double    Mrlu3_10_5;
+double    Mrlu3_10_6;
+double    Mrlu3_10_7;
+double    Mrlu3_10_8;
+double    Mrlu3_10_9;
+double    Mrlu3_10_10;
+double    Mrlu3_10_11;
+double    Mrlu3_10_12;
+double    Mrlu11_3;
+double    Mrlu3_11_4;
+double    Mrlu3_11_5;
+double    Mrlu3_11_6;
+double    Mrlu3_11_7;
+double    Mrlu3_11_8;
+double    Mrlu3_11_9;
+double    Mrlu3_11_10;
+double    Mrlu3_11_11;
+double    Mrlu3_11_12;
+double    Mrlu12_3;
+double    Mrlu3_12_4;
+double    Mrlu3_12_5;
+double    Mrlu3_12_6;
+double    Mrlu3_12_7;
+double    Mrlu3_12_8;
+double    Mrlu3_12_9;
+double    Mrlu3_12_10;
+double    Mrlu3_12_11;
+double    Mrlu3_12_12;
+double    Mrlu5_4;
+double    Mrlu5_5;
+double    Mrlu5_6;
+double    Mrlu5_7;
+double    Mrlu5_8;
+double    Mrlu5_9;
+double    Mrlu5_10;
+double    Mrlu5_11;
+double    Mrlu5_12;
+double    Mrlu6_4;
+double    Mrlu4_6_5;
+double    Mrlu4_6_6;
+double    Mrlu4_6_7;
+double    Mrlu4_6_8;
+double    Mrlu4_6_9;
+double    Mrlu4_6_10;
+double    Mrlu4_6_11;
+double    Mrlu4_6_12;
+double    Mrlu7_4;
+double    Mrlu4_7_5;
+double    Mrlu4_7_6;
+double    Mrlu4_7_7;
+double    Mrlu4_7_8;
+double    Mrlu4_7_9;
+double    Mrlu4_7_10;
+double    Mrlu4_7_11;
+double    Mrlu4_7_12;
+double    Mrlu8_4;
+double    Mrlu4_8_5;
+double    Mrlu4_8_6;
+double    Mrlu4_8_7;
+double    Mrlu4_8_8;
+double    Mrlu4_8_9;
+double    Mrlu4_8_10;
+double    Mrlu4_8_11;
+double    Mrlu4_8_12;
+double    Mrlu9_4;
+double    Mrlu4_9_5;
+double    Mrlu4_9_6;
+double    Mrlu4_9_7;
+double    Mrlu4_9_8;
+double    Mrlu4_9_9;
+double    Mrlu4_9_10;
+double    Mrlu4_9_11;
+double    Mrlu4_9_12;
+double    Mrlu10_4;
+double    Mrlu4_10_5;
+double    Mrlu4_10_6;
+double    Mrlu4_10_7;
+double    Mrlu4_10_8;
+double    Mrlu4_10_9;
+double    Mrlu4_10_10;
+double    Mrlu4_10_11;
+double    Mrlu4_10_12;
+double    Mrlu11_4;
+double    Mrlu4_11_5;
+double    Mrlu4_11_6;
+double    Mrlu4_11_7;
+double    Mrlu4_11_8;
+double    Mrlu4_11_9;
+double    Mrlu4_11_10;
+double    Mrlu4_11_11;
+double    Mrlu4_11_12;
+double    Mrlu12_4;
+double    Mrlu4_12_5;
+double    Mrlu4_12_6;
+double    Mrlu4_12_7;
+double    Mrlu4_12_8;
+double    Mrlu4_12_9;
+double    Mrlu4_12_10;
+double    Mrlu4_12_11;
+double    Mrlu4_12_12;
+double    Mrlu6_5;
+double    Mrlu6_6;
+double    Mrlu6_7;
+double    Mrlu6_8;
+double    Mrlu6_9;
+double    Mrlu6_10;
+double    Mrlu6_11;
+double    Mrlu6_12;
+double    Mrlu7_5;
+double    Mrlu5_7_6;
+double    Mrlu5_7_7;
+double    Mrlu5_7_8;
+double    Mrlu5_7_9;
+double    Mrlu5_7_10;
+double    Mrlu5_7_11;
+double    Mrlu5_7_12;
+double    Mrlu8_5;
+double    Mrlu5_8_6;
+double    Mrlu5_8_7;
+double    Mrlu5_8_8;
+double    Mrlu5_8_9;
+double    Mrlu5_8_10;
+double    Mrlu5_8_11;
+double    Mrlu5_8_12;
+double    Mrlu9_5;
+double    Mrlu5_9_6;
+double    Mrlu5_9_7;
+double    Mrlu5_9_8;
+double    Mrlu5_9_9;
+double    Mrlu5_9_10;
+double    Mrlu5_9_11;
+double    Mrlu5_9_12;
+double    Mrlu10_5;
+double    Mrlu5_10_6;
+double    Mrlu5_10_7;
+double    Mrlu5_10_8;
+double    Mrlu5_10_9;
+double    Mrlu5_10_10;
+double    Mrlu5_10_11;
+double    Mrlu5_10_12;
+double    Mrlu11_5;
+double    Mrlu5_11_6;
+double    Mrlu5_11_7;
+double    Mrlu5_11_8;
+double    Mrlu5_11_9;
+double    Mrlu5_11_10;
+double    Mrlu5_11_11;
+double    Mrlu5_11_12;
+double    Mrlu12_5;
+double    Mrlu5_12_6;
+double    Mrlu5_12_7;
+double    Mrlu5_12_8;
+double    Mrlu5_12_9;
+double    Mrlu5_12_10;
+double    Mrlu5_12_11;
+double    Mrlu5_12_12;
+double    Mrlu7_6;
+double    Mrlu7_7;
+double    Mrlu7_8;
+double    Mrlu7_9;
+double    Mrlu7_10;
+double    Mrlu7_11;
+double    Mrlu7_12;
+double    Mrlu8_6;
+double    Mrlu6_8_7;
+double    Mrlu6_8_8;
+double    Mrlu6_8_9;
+double    Mrlu6_8_10;
+double    Mrlu6_8_11;
+double    Mrlu6_8_12;
+double    Mrlu9_6;
+double    Mrlu6_9_7;
+double    Mrlu6_9_8;
+double    Mrlu6_9_9;
+double    Mrlu6_9_10;
+double    Mrlu6_9_11;
+double    Mrlu6_9_12;
+double    Mrlu10_6;
+double    Mrlu6_10_7;
+double    Mrlu6_10_8;
+double    Mrlu6_10_9;
+double    Mrlu6_10_10;
+double    Mrlu6_10_11;
+double    Mrlu6_10_12;
+double    Mrlu11_6;
+double    Mrlu6_11_7;
+double    Mrlu6_11_8;
+double    Mrlu6_11_9;
+double    Mrlu6_11_10;
+double    Mrlu6_11_11;
+double    Mrlu6_11_12;
+double    Mrlu12_6;
+double    Mrlu6_12_7;
+double    Mrlu6_12_8;
+double    Mrlu6_12_9;
+double    Mrlu6_12_10;
+double    Mrlu6_12_11;
+double    Mrlu6_12_12;
+double    Mrlu8_7;
+double    Mrlu8_8;
+double    Mrlu8_9;
+double    Mrlu8_10;
+double    Mrlu8_11;
+double    Mrlu8_12;
+double    Mrlu9_7;
+double    Mrlu7_9_8;
+double    Mrlu7_9_9;
+double    Mrlu7_9_10;
+double    Mrlu7_9_11;
+double    Mrlu7_9_12;
+double    Mrlu10_7;
+double    Mrlu7_10_8;
+double    Mrlu7_10_9;
+double    Mrlu7_10_10;
+double    Mrlu7_10_11;
+double    Mrlu7_10_12;
+double    Mrlu11_7;
+double    Mrlu7_11_8;
+double    Mrlu7_11_9;
+double    Mrlu7_11_10;
+double    Mrlu7_11_11;
+double    Mrlu7_11_12;
+double    Mrlu12_7;
+double    Mrlu7_12_8;
+double    Mrlu7_12_9;
+double    Mrlu7_12_10;
+double    Mrlu7_12_11;
+double    Mrlu7_12_12;
+double    Mrlu9_8;
+double    Mrlu9_9;
+double    Mrlu9_10;
+double    Mrlu9_11;
+double    Mrlu9_12;
+double    Mrlu10_8;
+double    Mrlu8_10_9;
+double    Mrlu8_10_10;
+double    Mrlu8_10_11;
+double    Mrlu8_10_12;
+double    Mrlu11_8;
+double    Mrlu8_11_9;
+double    Mrlu8_11_10;
+double    Mrlu8_11_11;
+double    Mrlu8_11_12;
+double    Mrlu12_8;
+double    Mrlu8_12_9;
+double    Mrlu8_12_10;
+double    Mrlu8_12_11;
+double    Mrlu8_12_12;
+double    Mrlu10_9;
+double    Mrlu10_10;
+double    Mrlu10_11;
+double    Mrlu10_12;
+double    Mrlu11_9;
+double    Mrlu9_11_10;
+double    Mrlu9_11_11;
+double    Mrlu9_11_12;
+double    Mrlu12_9;
+double    Mrlu9_12_10;
+double    Mrlu9_12_11;
+double    Mrlu9_12_12;
+double    Mrlu11_10;
+double    Mrlu11_11;
+double    Mrlu11_12;
+double    Mrlu12_10;
+double    Mrlu10_12_11;
+double    Mrlu10_12_12;
+double    Mrlu12_11;
+double    Mrlu12_12;
+double    qppuy2;
+double    qppuy3;
+double    qppuy4;
+double    qppuy5;
+double    qppuy6;
+double    qppuy7;
+double    qppuy8;
+double    qppuy9;
+double    qppuy10;
+double    qppuy11;
+double    qppuy12;
+double    qppux12;
+double    qppux11;
+double    qppux10;
+double    qppux9;
+double    qppux8;
+double    qppux7;
+double    qppux6;
+double    qppux5;
+double    qppux4;
+double    qppux3;
+double    qppux2;
+double    qppux1;
diff --git a/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.c b/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.c
new file mode 100644
index 0000000..316b432
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.c
@@ -0,0 +1,529 @@
+//
+//-------------------------------------------------------------
+//
+//	ROBOTRAN - Version 6.6 (build : february 22, 2008)
+//
+//	Copyright 
+//	Universite catholique de Louvain 
+//	Departement de Mecanique 
+//	Unite de Production Mecanique et Machines 
+//	2, Place du Levant 
+//	1348 Louvain-la-Neuve 
+//	http://www.robotran.be// 
+//
+//	==> Generation Date : Fri Jan 27 19:08:04 2017
+//
+//	==> Project name : coman_short_feet_simp
+//	==> using XML input file 
+//
+//	==> Number of joints : 12
+//
+//	==> Function : F 1 : Direct Dynamics (Semi-Explicit formulation) : RNEA
+//	==> Flops complexity : 1250
+//
+//	==> Generation Time :  0.020 seconds
+//	==> Post-Processing :  0.020 seconds
+//
+//-------------------------------------------------------------
+//
+ 
+#include <math.h> 
+
+#include "mbs_data.h"
+#include "mbs_project_interface.h"
+ 
+void mbs_dirdyna(double **M,double *c,
+MbsData *s, double tsim)
+
+// double M[12][12];
+// double c[12];
+{ 
+ 
+#include "mbs_dirdyna_coman_short_feet_simp.h" 
+#define q s->q 
+#define qd s->qd 
+#define qdd s->qdd 
+ 
+ 
+
+// === begin imp_aux === 
+
+// === end imp_aux === 
+
+// ===== BEGIN task 0 ===== 
+
+// = = Block_0_0_0_0_0_1 = = 
+ 
+// Trigonometric Variables  
+
+  C3 = cos(q[3]);
+  S3 = sin(q[3]);
+
+// = = Block_0_0_0_0_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  C4 = cos(q[4]);
+  S4 = sin(q[4]);
+  C5 = cos(q[5]);
+  S5 = sin(q[5]);
+  C6 = cos(q[6]);
+  S6 = sin(q[6]);
+
+// = = Block_0_0_0_0_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  C7 = cos(q[7]);
+  S7 = sin(q[7]);
+  C8 = cos(q[8]);
+  S8 = sin(q[8]);
+  C9 = cos(q[9]);
+  S9 = sin(q[9]);
+
+// = = Block_0_0_0_0_0_4 = = 
+ 
+// Trigonometric Variables  
+
+  C10 = cos(q[10]);
+  S10 = sin(q[10]);
+
+// = = Block_0_0_0_0_0_5 = = 
+ 
+// Trigonometric Variables  
+
+  C11 = cos(q[11]);
+  S11 = sin(q[11]);
+
+// = = Block_0_0_0_0_0_6 = = 
+ 
+// Trigonometric Variables  
+
+  C12 = cos(q[12]);
+  S12 = sin(q[12]);
+
+// = = Block_0_1_0_0_0_1 = = 
+ 
+// Forward Kinematics 
+
+  BS13 = -qd[3]*qd[3];
+  BS93 = -qd[3]*qd[3];
+  AlF13 = s->g[3]*S3;
+  AlF33 = -s->g[3]*C3;
+
+// = = Block_0_1_0_1_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4 = C3*S4+S3*C4;
+  C3p4 = C3*C4-S3*S4;
+  S3p4p5 = C3p4*S5+S3p4*C5;
+  C3p4p5 = C3p4*C5-S3p4*S5;
+  S3p4p5p6 = C3p4p5*S6+S3p4p5*C6;
+  C3p4p5p6 = C3p4p5*C6-S3p4p5*S6;
+ 
+// Forward Kinematics 
+
+  OM24 = qd[3]+qd[4];
+  BS94 = -OM24*OM24;
+  AlF14 = s->g[3]*S3p4;
+  AlF34 = -s->g[3]*C3p4;
+  OM25 = qd[5]+OM24;
+  BS95 = -OM25*OM25;
+  AlF15 = AlF14*C5-S5*(AlF34+BS94*s->dpt[3][4]);
+  AlF35 = AlF14*S5+C5*(AlF34+BS94*s->dpt[3][4]);
+  AlM15_3 = s->dpt[3][4]*C5;
+  AlM35_3 = s->dpt[3][4]*S5;
+  AlM15_4 = s->dpt[3][4]*C5;
+  AlM35_4 = s->dpt[3][4]*S5;
+  OM26 = qd[6]+OM25;
+
+// = = Block_0_1_0_1_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7 = C3*S7+S3*C7;
+  C3p7 = C3*C7-S3*S7;
+  S3p7p8 = C3p7*S8+S3p7*C8;
+  C3p7p8 = C3p7*C8-S3p7*S8;
+  S3p7p8p9 = C3p7p8*S9+S3p7p8*C9;
+  C3p7p8p9 = C3p7p8*C9-S3p7p8*S9;
+ 
+// Forward Kinematics 
+
+  OM27 = qd[3]+qd[7];
+  BS97 = -OM27*OM27;
+  AlF17 = s->g[3]*S3p7;
+  AlF37 = -s->g[3]*C3p7;
+  OM28 = qd[8]+OM27;
+  BS98 = -OM28*OM28;
+  AlF18 = AlF17*C8-S8*(AlF37+BS97*s->dpt[3][7]);
+  AlF38 = AlF17*S8+C8*(AlF37+BS97*s->dpt[3][7]);
+  AlM18_3 = s->dpt[3][7]*C8;
+  AlM38_3 = s->dpt[3][7]*S8;
+  AlM18_7 = s->dpt[3][7]*C8;
+  AlM38_7 = s->dpt[3][7]*S8;
+  OM29 = qd[9]+OM28;
+
+// = = Block_0_1_0_1_0_4 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3 = C10*S3+S10*C3;
+  C10p3 = C10*C3-S10*S3;
+ 
+// Forward Kinematics 
+
+  OM210 = qd[10]+qd[3];
+  BS110 = -OM210*OM210;
+  BS910 = -OM210*OM210;
+  AlF110 = C10*(AlF13+BS13*s->dpt[1][3])-S10*(AlF33+BS93*s->dpt[3][3]);
+  AlF310 = C10*(AlF33+BS93*s->dpt[3][3])+S10*(AlF13+BS13*s->dpt[1][3]);
+  AlM110_3 = s->dpt[1][3]*S10+s->dpt[3][3]*C10;
+  AlM310_3 = -(s->dpt[1][3]*C10-s->dpt[3][3]*S10);
+
+// = = Block_0_1_0_2_0_5 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3p11 = C10p3*S11+S10p3*C11;
+  C10p3p11 = C10p3*C11-S10p3*S11;
+ 
+// Forward Kinematics 
+
+  OM211 = qd[11]+OM210;
+
+// = = Block_0_1_0_2_0_6 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3p12 = C10p3*S12+S10p3*C12;
+  C10p3p12 = C10p3*C12-S10p3*S12;
+ 
+// Forward Kinematics 
+
+  OM212 = qd[12]+OM210;
+
+// = = Block_0_2_0_1_0_5 = = 
+ 
+// Backward Dynamics 
+
+  FA111 = -(s->frc[1][11]+s->m[11]*(OM211*OM211*s->l[1][11]-C11*(AlF110+BS110*s->dpt[1][10])+S11*(AlF310+BS910*
+ s->dpt[3][10])));
+  FA311 = -(s->frc[3][11]+s->m[11]*(OM211*OM211*s->l[3][11]-C11*(AlF310+BS910*s->dpt[3][10])-S11*(AlF110+BS110*
+ s->dpt[1][10])));
+  CF211 = -(s->trq[2][11]-FA111*s->l[3][11]+FA311*s->l[1][11]);
+  CM211_1 = -s->m[11]*(s->l[1][11]*S10p3p11-s->l[3][11]*C10p3p11);
+  CM211_2 = -s->m[11]*(s->l[1][11]*C10p3p11+s->l[3][11]*S10p3p11);
+  FB111_3 = s->m[11]*(s->l[3][11]+C11*(AlM110_3+s->dpt[3][10])-S11*(AlM310_3-s->dpt[1][10]));
+  FB311_3 = s->m[11]*(C11*(AlM310_3-s->dpt[1][10])+S11*(AlM110_3+s->dpt[3][10])-s->l[1][11]);
+  CM211_3 = s->In[5][11]+FB111_3*s->l[3][11]-FB311_3*s->l[1][11];
+  FB111_10 = s->m[11]*(s->l[3][11]+s->dpt[1][10]*S11+s->dpt[3][10]*C11);
+  FB311_10 = -s->m[11]*(s->l[1][11]+s->dpt[1][10]*C11-s->dpt[3][10]*S11);
+  CM211_10 = s->In[5][11]+FB111_10*s->l[3][11]-FB311_10*s->l[1][11];
+  CM211_11 = s->In[5][11]+s->m[11]*s->l[1][11]*s->l[1][11]+s->m[11]*s->l[3][11]*s->l[3][11];
+
+// = = Block_0_2_0_1_0_6 = = 
+ 
+// Backward Dynamics 
+
+  FA112 = -(s->frc[1][12]+s->m[12]*(OM212*OM212*s->l[1][12]-C12*(AlF110+BS110*s->dpt[1][11])+S12*(AlF310+BS910*
+ s->dpt[3][11])));
+  FA312 = -(s->frc[3][12]+s->m[12]*(OM212*OM212*s->l[3][12]-C12*(AlF310+BS910*s->dpt[3][11])-S12*(AlF110+BS110*
+ s->dpt[1][11])));
+  CF212 = -(s->trq[2][12]-FA112*s->l[3][12]+FA312*s->l[1][12]);
+  CM212_1 = -s->m[12]*(s->l[1][12]*S10p3p12-s->l[3][12]*C10p3p12);
+  CM212_2 = -s->m[12]*(s->l[1][12]*C10p3p12+s->l[3][12]*S10p3p12);
+  FB112_3 = s->m[12]*(s->l[3][12]+C12*(AlM110_3+s->dpt[3][11])-S12*(AlM310_3-s->dpt[1][11]));
+  FB312_3 = s->m[12]*(C12*(AlM310_3-s->dpt[1][11])+S12*(AlM110_3+s->dpt[3][11])-s->l[1][12]);
+  CM212_3 = s->In[5][12]+FB112_3*s->l[3][12]-FB312_3*s->l[1][12];
+  FB112_10 = s->m[12]*(s->l[3][12]+s->dpt[1][11]*S12+s->dpt[3][11]*C12);
+  FB312_10 = -s->m[12]*(s->l[1][12]+s->dpt[1][11]*C12-s->dpt[3][11]*S12);
+  CM212_10 = s->In[5][12]+FB112_10*s->l[3][12]-FB312_10*s->l[1][12];
+  CM212_12 = s->In[5][12]+s->m[12]*s->l[1][12]*s->l[1][12]+s->m[12]*s->l[3][12]*s->l[3][12];
+
+// = = Block_0_2_0_2_0_2 = = 
+ 
+// Backward Dynamics 
+
+  FA16 = -(s->frc[1][6]-s->m[6]*(AlF15*C6-OM26*OM26*s->l[1][6]-S6*(AlF35+BS95*s->dpt[3][5])));
+  FA36 = -(s->frc[3][6]-s->m[6]*(AlF15*S6-OM26*OM26*s->l[3][6]+C6*(AlF35+BS95*s->dpt[3][5])));
+  CF26 = -(s->trq[2][6]-FA16*s->l[3][6]+FA36*s->l[1][6]);
+  FB16_1 = s->m[6]*C3p4p5p6;
+  FB36_1 = s->m[6]*S3p4p5p6;
+  CM26_1 = FB16_1*s->l[3][6]-FB36_1*s->l[1][6];
+  FB16_2 = -s->m[6]*S3p4p5p6;
+  FB36_2 = s->m[6]*C3p4p5p6;
+  CM26_2 = FB16_2*s->l[3][6]-FB36_2*s->l[1][6];
+  FB16_3 = s->m[6]*(s->l[3][6]-AlM35_3*S6+C6*(AlM15_3+s->dpt[3][5]));
+  FB36_3 = s->m[6]*(AlM35_3*C6+S6*(AlM15_3+s->dpt[3][5])-s->l[1][6]);
+  CM26_3 = s->In[5][6]+FB16_3*s->l[3][6]-FB36_3*s->l[1][6];
+  FB16_4 = s->m[6]*(s->l[3][6]-AlM35_4*S6+C6*(AlM15_4+s->dpt[3][5]));
+  FB36_4 = s->m[6]*(AlM35_4*C6+S6*(AlM15_4+s->dpt[3][5])-s->l[1][6]);
+  CM26_4 = s->In[5][6]+FB16_4*s->l[3][6]-FB36_4*s->l[1][6];
+  FB16_5 = s->m[6]*(s->l[3][6]+s->dpt[3][5]*C6);
+  FB36_5 = s->m[6]*(s->dpt[3][5]*S6-s->l[1][6]);
+  CM26_5 = s->In[5][6]+FB16_5*s->l[3][6]-FB36_5*s->l[1][6];
+  CM26_6 = s->In[5][6]+s->m[6]*s->l[1][6]*s->l[1][6]+s->m[6]*s->l[3][6]*s->l[3][6];
+  FA15 = -(s->frc[1][5]-s->m[5]*(AlF15-OM25*OM25*s->l[1][5]));
+  FA35 = -(s->frc[3][5]-s->m[5]*(AlF35+BS95*s->l[3][5]));
+  FF15 = FA15+FA16*C6+FA36*S6;
+  FF35 = FA35-FA16*S6+FA36*C6;
+  CF25 = -(s->trq[2][5]-CF26-FA15*s->l[3][5]+FA35*s->l[1][5]-s->dpt[3][5]*(FA16*C6+FA36*S6));
+  FB15_1 = s->m[5]*C3p4p5;
+  FB35_1 = s->m[5]*S3p4p5;
+  CM25_1 = CM26_1+FB15_1*s->l[3][5]-FB35_1*s->l[1][5]+s->dpt[3][5]*(FB16_1*C6+FB36_1*S6);
+  FB15_2 = -s->m[5]*S3p4p5;
+  FB35_2 = s->m[5]*C3p4p5;
+  CM25_2 = CM26_2+FB15_2*s->l[3][5]-FB35_2*s->l[1][5]+s->dpt[3][5]*(FB16_2*C6+FB36_2*S6);
+  FB15_3 = s->m[5]*(AlM15_3+s->l[3][5]);
+  FB35_3 = s->m[5]*(AlM35_3-s->l[1][5]);
+  CM25_3 = s->In[5][5]+CM26_3+FB15_3*s->l[3][5]-FB35_3*s->l[1][5]+s->dpt[3][5]*(FB16_3*C6+FB36_3*S6);
+  FB15_4 = s->m[5]*(AlM15_4+s->l[3][5]);
+  FB35_4 = s->m[5]*(AlM35_4-s->l[1][5]);
+  CM25_4 = s->In[5][5]+CM26_4+FB15_4*s->l[3][5]-FB35_4*s->l[1][5]+s->dpt[3][5]*(FB16_4*C6+FB36_4*S6);
+  CM25_5 = s->In[5][5]+CM26_5+s->m[5]*s->l[1][5]*s->l[1][5]+s->m[5]*s->l[3][5]*s->l[3][5]+s->dpt[3][5]*(FB16_5*C6+FB36_5
+ *S6);
+  FA14 = -(s->frc[1][4]-s->m[4]*(AlF14-OM24*OM24*s->l[1][4]));
+  FA34 = -(s->frc[3][4]-s->m[4]*(AlF34+BS94*s->l[3][4]));
+  FF14 = FA14+FF15*C5+FF35*S5;
+  FF34 = FA34-FF15*S5+FF35*C5;
+  CF24 = -(s->trq[2][4]-CF25-FA14*s->l[3][4]+FA34*s->l[1][4]-s->dpt[3][4]*(FF15*C5+FF35*S5));
+  CM24_1 = CM25_1-s->m[4]*s->l[1][4]*S3p4+s->m[4]*s->l[3][4]*C3p4+s->dpt[3][4]*(C5*(FB15_1+s->m[6]*C3p4p5)+S5*(FB35_1+
+ s->m[6]*S3p4p5));
+  FB14_2 = -s->m[4]*S3p4;
+  FB34_2 = s->m[4]*C3p4;
+  CM24_2 = CM25_2+FB14_2*s->l[3][4]-FB34_2*s->l[1][4]+s->dpt[3][4]*(C5*(FB15_2-s->m[6]*S3p4p5)+S5*(FB35_2+s->m[6]*C3p4p5
+ ));
+  CM24_3 = s->In[5][4]+CM25_3+s->m[4]*s->l[1][4]*s->l[1][4]+s->m[4]*s->l[3][4]*s->l[3][4]+s->dpt[3][4]*(C5*(FB15_3+
+ FB16_3*C6+FB36_3*S6)+S5*(FB35_3-FB16_3*S6+FB36_3*C6));
+  CM24_4 = s->In[5][4]+CM25_4+s->m[4]*s->l[1][4]*s->l[1][4]+s->m[4]*s->l[3][4]*s->l[3][4]+s->dpt[3][4]*(C5*(FB15_4+
+ FB16_4*C6+FB36_4*S6)+S5*(FB35_4-FB16_4*S6+FB36_4*C6));
+
+// = = Block_0_2_0_2_0_3 = = 
+ 
+// Backward Dynamics 
+
+  FA19 = -(s->frc[1][9]-s->m[9]*(AlF18*C9-OM29*OM29*s->l[1][9]-S9*(AlF38+BS98*s->dpt[3][8])));
+  FA39 = -(s->frc[3][9]-s->m[9]*(AlF18*S9-OM29*OM29*s->l[3][9]+C9*(AlF38+BS98*s->dpt[3][8])));
+  CF29 = -(s->trq[2][9]-FA19*s->l[3][9]+FA39*s->l[1][9]);
+  FB19_1 = s->m[9]*C3p7p8p9;
+  FB39_1 = s->m[9]*S3p7p8p9;
+  CM29_1 = FB19_1*s->l[3][9]-FB39_1*s->l[1][9];
+  FB19_2 = -s->m[9]*S3p7p8p9;
+  FB39_2 = s->m[9]*C3p7p8p9;
+  CM29_2 = FB19_2*s->l[3][9]-FB39_2*s->l[1][9];
+  FB19_3 = s->m[9]*(s->l[3][9]-AlM38_3*S9+C9*(AlM18_3+s->dpt[3][8]));
+  FB39_3 = s->m[9]*(AlM38_3*C9+S9*(AlM18_3+s->dpt[3][8])-s->l[1][9]);
+  CM29_3 = s->In[5][9]+FB19_3*s->l[3][9]-FB39_3*s->l[1][9];
+  FB19_7 = s->m[9]*(s->l[3][9]-AlM38_7*S9+C9*(AlM18_7+s->dpt[3][8]));
+  FB39_7 = s->m[9]*(AlM38_7*C9+S9*(AlM18_7+s->dpt[3][8])-s->l[1][9]);
+  CM29_7 = s->In[5][9]+FB19_7*s->l[3][9]-FB39_7*s->l[1][9];
+  FB19_8 = s->m[9]*(s->l[3][9]+s->dpt[3][8]*C9);
+  FB39_8 = s->m[9]*(s->dpt[3][8]*S9-s->l[1][9]);
+  CM29_8 = s->In[5][9]+FB19_8*s->l[3][9]-FB39_8*s->l[1][9];
+  CM29_9 = s->In[5][9]+s->m[9]*s->l[1][9]*s->l[1][9]+s->m[9]*s->l[3][9]*s->l[3][9];
+  FA18 = -(s->frc[1][8]-s->m[8]*(AlF18-OM28*OM28*s->l[1][8]));
+  FA38 = -(s->frc[3][8]-s->m[8]*(AlF38+BS98*s->l[3][8]));
+  FF18 = FA18+FA19*C9+FA39*S9;
+  FF38 = FA38-FA19*S9+FA39*C9;
+  CF28 = -(s->trq[2][8]-CF29-FA18*s->l[3][8]+FA38*s->l[1][8]-s->dpt[3][8]*(FA19*C9+FA39*S9));
+  FB18_1 = s->m[8]*C3p7p8;
+  FB38_1 = s->m[8]*S3p7p8;
+  CM28_1 = CM29_1+FB18_1*s->l[3][8]-FB38_1*s->l[1][8]+s->dpt[3][8]*(FB19_1*C9+FB39_1*S9);
+  FB18_2 = -s->m[8]*S3p7p8;
+  FB38_2 = s->m[8]*C3p7p8;
+  CM28_2 = CM29_2+FB18_2*s->l[3][8]-FB38_2*s->l[1][8]+s->dpt[3][8]*(FB19_2*C9+FB39_2*S9);
+  FB18_3 = s->m[8]*(AlM18_3+s->l[3][8]);
+  FB38_3 = s->m[8]*(AlM38_3-s->l[1][8]);
+  CM28_3 = s->In[5][8]+CM29_3+FB18_3*s->l[3][8]-FB38_3*s->l[1][8]+s->dpt[3][8]*(FB19_3*C9+FB39_3*S9);
+  FB18_7 = s->m[8]*(AlM18_7+s->l[3][8]);
+  FB38_7 = s->m[8]*(AlM38_7-s->l[1][8]);
+  CM28_7 = s->In[5][8]+CM29_7+FB18_7*s->l[3][8]-FB38_7*s->l[1][8]+s->dpt[3][8]*(FB19_7*C9+FB39_7*S9);
+  CM28_8 = s->In[5][8]+CM29_8+s->m[8]*s->l[1][8]*s->l[1][8]+s->m[8]*s->l[3][8]*s->l[3][8]+s->dpt[3][8]*(FB19_8*C9+FB39_8
+ *S9);
+  FA17 = -(s->frc[1][7]-s->m[7]*(AlF17-OM27*OM27*s->l[1][7]));
+  FA37 = -(s->frc[3][7]-s->m[7]*(AlF37+BS97*s->l[3][7]));
+  FF17 = FA17+FF18*C8+FF38*S8;
+  FF37 = FA37-FF18*S8+FF38*C8;
+  CF27 = -(s->trq[2][7]-CF28-FA17*s->l[3][7]+FA37*s->l[1][7]-s->dpt[3][7]*(FF18*C8+FF38*S8));
+  CM27_1 = CM28_1-s->m[7]*s->l[1][7]*S3p7+s->m[7]*s->l[3][7]*C3p7+s->dpt[3][7]*(C8*(FB18_1+s->m[9]*C3p7p8)+S8*(FB38_1+
+ s->m[9]*S3p7p8));
+  FB17_2 = -s->m[7]*S3p7;
+  FB37_2 = s->m[7]*C3p7;
+  CM27_2 = CM28_2+FB17_2*s->l[3][7]-FB37_2*s->l[1][7]+s->dpt[3][7]*(C8*(FB18_2-s->m[9]*S3p7p8)+S8*(FB38_2+s->m[9]*C3p7p8
+ ));
+  CM27_3 = s->In[5][7]+CM28_3+s->m[7]*s->l[1][7]*s->l[1][7]+s->m[7]*s->l[3][7]*s->l[3][7]+s->dpt[3][7]*(C8*(FB18_3+
+ FB19_3*C9+FB39_3*S9)+S8*(FB38_3-FB19_3*S9+FB39_3*C9));
+  CM27_7 = s->In[5][7]+CM28_7+s->m[7]*s->l[1][7]*s->l[1][7]+s->m[7]*s->l[3][7]*s->l[3][7]+s->dpt[3][7]*(C8*(FB18_7+
+ FB19_7*C9+FB39_7*S9)+S8*(FB38_7-FB19_7*S9+FB39_7*C9));
+
+// = = Block_0_2_0_2_0_4 = = 
+ 
+// Backward Dynamics 
+
+  FA110 = -(s->frc[1][10]-s->m[10]*(AlF110+BS110*s->l[1][10]));
+  FA310 = -(s->frc[3][10]-s->m[10]*(AlF310+BS910*s->l[3][10]));
+  FF110 = FA110+FA111*C11+FA112*C12+FA311*S11+FA312*S12;
+  FF310 = FA310-FA111*S11-FA112*S12+FA311*C11+FA312*C12;
+  CF210 = -(s->trq[2][10]-CF211-CF212-FA110*s->l[3][10]+FA310*s->l[1][10]-s->dpt[1][10]*(FA111*S11-FA311*C11)-
+ s->dpt[1][11]*(FA112*S12-FA312*C12)-s->dpt[3][10]*(FA111*C11+FA311*S11)-s->dpt[3][11]*(FA112*C12+FA312*S12));
+  FB110_1 = s->m[10]*C10p3;
+  FB310_1 = s->m[10]*S10p3;
+  FM110_1 = FB110_1+s->m[11]*C10p3+s->m[12]*C10p3;
+  FM310_1 = FB310_1+s->m[11]*S10p3+s->m[12]*S10p3;
+  CM210_1 = -(s->m[11]*(s->dpt[1][10]*S10p3-s->dpt[3][10]*C10p3+s->l[1][11]*S10p3p11-s->l[3][11]*C10p3p11)+s->m[12]*(
+ s->dpt[1][11]*S10p3-s->dpt[3][11]*C10p3+s->l[1][12]*S10p3p12-s->l[3][12]*C10p3p12)-FB110_1*s->l[3][10]+FB310_1*s->l[1][10]);
+  FB110_2 = -s->m[10]*S10p3;
+  FB310_2 = s->m[10]*C10p3;
+  FM110_2 = FB110_2-s->m[11]*S10p3-s->m[12]*S10p3;
+  FM310_2 = FB310_2+s->m[11]*C10p3+s->m[12]*C10p3;
+  CM210_2 = -(s->m[11]*(s->dpt[1][10]*C10p3+s->dpt[3][10]*S10p3+s->l[1][11]*C10p3p11+s->l[3][11]*S10p3p11)+s->m[12]*(
+ s->dpt[1][11]*C10p3+s->dpt[3][11]*S10p3+s->l[1][12]*C10p3p12+s->l[3][12]*S10p3p12)-FB110_2*s->l[3][10]+FB310_2*s->l[1][10]);
+  FB110_3 = s->m[10]*(AlM110_3+s->l[3][10]);
+  FB310_3 = s->m[10]*(AlM310_3-s->l[1][10]);
+  FM110_3 = FB110_3+FB111_3*C11+FB112_3*C12+FB311_3*S11+FB312_3*S12;
+  FM310_3 = FB310_3-FB111_3*S11-FB112_3*S12+FB311_3*C11+FB312_3*C12;
+  CM210_3 = s->In[5][10]+CM211_3+CM212_3+FB110_3*s->l[3][10]-FB310_3*s->l[1][10]+s->dpt[1][10]*(FB111_3*S11-FB311_3*C11)
+ +s->dpt[1][11]*(FB112_3*S12-FB312_3*C12)+s->dpt[3][10]*(FB111_3*C11+FB311_3*S11)+s->dpt[3][11]*(FB112_3*C12+FB312_3*S12);
+  CM210_10 = s->In[5][10]+CM211_10+CM212_10+s->m[10]*s->l[1][10]*s->l[1][10]+s->m[10]*s->l[3][10]*s->l[3][10]+
+ s->dpt[1][10]*(FB111_10*S11-FB311_10*C11)+s->dpt[1][11]*(FB112_10*S12-FB312_10*C12)+s->dpt[3][10]*(FB111_10*C11+FB311_10*S11
+ )+s->dpt[3][11]*(FB112_10*C12+FB312_10*S12);
+
+// = = Block_0_2_0_3_0_1 = = 
+ 
+// Backward Dynamics 
+
+  FA13 = -(s->frc[1][3]-s->m[3]*(AlF13+BS13*s->l[1][3]));
+  FA33 = -(s->frc[3][3]-s->m[3]*(AlF33+BS93*s->l[3][3]));
+  FF13 = FA13+FF110*C10+FF14*C4+FF17*C7+FF310*S10+FF34*S4+FF37*S7;
+  FF33 = FA33-FF110*S10-FF14*S4-FF17*S7+FF310*C10+FF34*C4+FF37*C7;
+  CF23 = -(s->trq[2][3]-CF210-CF24-CF27-FA13*s->l[3][3]+FA33*s->l[1][3]-s->dpt[1][3]*(FF110*S10-FF310*C10)-s->dpt[3][3]*
+ (FF110*C10+FF310*S10));
+  FB13_1 = s->m[3]*C3;
+  FB33_1 = s->m[3]*S3;
+  CM23_1 = CM210_1+CM24_1+CM27_1+FB13_1*s->l[3][3]-FB33_1*s->l[1][3]+s->dpt[1][3]*(FM110_1*S10-FM310_1*C10)+s->dpt[3][3]
+ *(FM110_1*C10+FM310_1*S10);
+  FB13_2 = -s->m[3]*S3;
+  FB33_2 = s->m[3]*C3;
+  CM23_2 = CM210_2+CM24_2+CM27_2+FB13_2*s->l[3][3]-FB33_2*s->l[1][3]+s->dpt[1][3]*(FM110_2*S10-FM310_2*C10)+s->dpt[3][3]
+ *(FM110_2*C10+FM310_2*S10);
+  CM23_3 = s->In[5][3]+CM210_3+CM24_3+CM27_3+s->m[3]*s->l[1][3]*s->l[1][3]+s->m[3]*s->l[3][3]*s->l[3][3]+s->dpt[1][3]*(
+ FM110_3*S10-FM310_3*C10)+s->dpt[3][3]*(FM110_3*C10+FM310_3*S10);
+  FF2_13 = FF13*C3+FF33*S3;
+  FF2_33 = -(FF13*S3-FF33*C3);
+  FM21_13 = C3*(FB13_1+C3*(s->m[10]+s->m[11]+s->m[12])+C3*(s->m[4]+s->m[5]+s->m[6])+C3*(s->m[7]+s->m[8]+s->m[9]))+S3*(
+ FB33_1+S3*(s->m[10]+s->m[11]+s->m[12])+S3*(s->m[4]+s->m[5]+s->m[6])+S3*(s->m[7]+s->m[8]+s->m[9]));
+  FM22_33 = C3*(FB33_2+C3*(s->m[10]+s->m[11]+s->m[12])+C3*(s->m[4]+s->m[5]+s->m[6])+C3*(s->m[7]+s->m[8]+s->m[9]))-S3*(
+ FB13_2+FM110_2*C10+FM310_2*S10+C4*(FB14_2-S3p4*(s->m[5]+s->m[6]))+S4*(FB34_2+C3p4*(s->m[5]+s->m[6]))+C7*(FB17_2-S3p7*(
+ s->m[8]+s->m[9]))+S7*(FB37_2+C3p7*(s->m[8]+s->m[9])));
+
+// = = Block_0_3_0_0_0_0 = = 
+ 
+// Symbolic Outputs  
+
+  M[1][1] = FM21_13;
+  M[1][3] = CM23_1;
+  M[1][4] = CM24_1;
+  M[1][5] = CM25_1;
+  M[1][6] = CM26_1;
+  M[1][7] = CM27_1;
+  M[1][8] = CM28_1;
+  M[1][9] = CM29_1;
+  M[1][10] = CM210_1;
+  M[1][11] = CM211_1;
+  M[1][12] = CM212_1;
+  M[2][2] = FM22_33;
+  M[2][3] = CM23_2;
+  M[2][4] = CM24_2;
+  M[2][5] = CM25_2;
+  M[2][6] = CM26_2;
+  M[2][7] = CM27_2;
+  M[2][8] = CM28_2;
+  M[2][9] = CM29_2;
+  M[2][10] = CM210_2;
+  M[2][11] = CM211_2;
+  M[2][12] = CM212_2;
+  M[3][1] = CM23_1;
+  M[3][2] = CM23_2;
+  M[3][3] = CM23_3;
+  M[3][4] = CM24_3;
+  M[3][5] = CM25_3;
+  M[3][6] = CM26_3;
+  M[3][7] = CM27_3;
+  M[3][8] = CM28_3;
+  M[3][9] = CM29_3;
+  M[3][10] = CM210_3;
+  M[3][11] = CM211_3;
+  M[3][12] = CM212_3;
+  M[4][1] = CM24_1;
+  M[4][2] = CM24_2;
+  M[4][3] = CM24_3;
+  M[4][4] = CM24_4;
+  M[4][5] = CM25_4;
+  M[4][6] = CM26_4;
+  M[5][1] = CM25_1;
+  M[5][2] = CM25_2;
+  M[5][3] = CM25_3;
+  M[5][4] = CM25_4;
+  M[5][5] = CM25_5;
+  M[5][6] = CM26_5;
+  M[6][1] = CM26_1;
+  M[6][2] = CM26_2;
+  M[6][3] = CM26_3;
+  M[6][4] = CM26_4;
+  M[6][5] = CM26_5;
+  M[6][6] = CM26_6;
+  M[7][1] = CM27_1;
+  M[7][2] = CM27_2;
+  M[7][3] = CM27_3;
+  M[7][7] = CM27_7;
+  M[7][8] = CM28_7;
+  M[7][9] = CM29_7;
+  M[8][1] = CM28_1;
+  M[8][2] = CM28_2;
+  M[8][3] = CM28_3;
+  M[8][7] = CM28_7;
+  M[8][8] = CM28_8;
+  M[8][9] = CM29_8;
+  M[9][1] = CM29_1;
+  M[9][2] = CM29_2;
+  M[9][3] = CM29_3;
+  M[9][7] = CM29_7;
+  M[9][8] = CM29_8;
+  M[9][9] = CM29_9;
+  M[10][1] = CM210_1;
+  M[10][2] = CM210_2;
+  M[10][3] = CM210_3;
+  M[10][10] = CM210_10;
+  M[10][11] = CM211_10;
+  M[10][12] = CM212_10;
+  M[11][1] = CM211_1;
+  M[11][2] = CM211_2;
+  M[11][3] = CM211_3;
+  M[11][10] = CM211_10;
+  M[11][11] = CM211_11;
+  M[12][1] = CM212_1;
+  M[12][2] = CM212_2;
+  M[12][3] = CM212_3;
+  M[12][10] = CM212_10;
+  M[12][12] = CM212_12;
+  c[1] = FF2_13;
+  c[2] = FF2_33;
+  c[3] = CF23;
+  c[4] = CF24;
+  c[5] = CF25;
+  c[6] = CF26;
+  c[7] = CF27;
+  c[8] = CF28;
+  c[9] = CF29;
+  c[10] = CF210;
+  c[11] = CF211;
+  c[12] = CF212;
+
+// ====== END Task 0 ====== 
+
+
+}
+ 
+
diff --git a/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.h b/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.h
new file mode 100644
index 0000000..5561a9e
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_dirdyna_coman_short_feet_simp.h
@@ -0,0 +1,234 @@
+double    C3;
+double    S3;
+double    C4;
+double    S4;
+double    C5;
+double    S5;
+double    C6;
+double    S6;
+double    C7;
+double    S7;
+double    C8;
+double    S8;
+double    C9;
+double    S9;
+double    C10;
+double    S10;
+double    C11;
+double    S11;
+double    C12;
+double    S12;
+double    BS13;
+double    BS93;
+double    AlF13;
+double    AlF33;
+double    OM24;
+double    BS94;
+double    S3p4;
+double    AlF14;
+double    C3p4;
+double    AlF34;
+double    OM25;
+double    BS95;
+double    AlF15;
+double    AlF35;
+double    S3p4p5;
+double    C3p4p5;
+double    AlM15_3;
+double    AlM35_3;
+double    AlM15_4;
+double    AlM35_4;
+double    OM26;
+double    S3p4p5p6;
+double    C3p4p5p6;
+double    OM27;
+double    BS97;
+double    S3p7;
+double    AlF17;
+double    C3p7;
+double    AlF37;
+double    OM28;
+double    BS98;
+double    AlF18;
+double    AlF38;
+double    S3p7p8;
+double    C3p7p8;
+double    AlM18_3;
+double    AlM38_3;
+double    AlM18_7;
+double    AlM38_7;
+double    OM29;
+double    S3p7p8p9;
+double    C3p7p8p9;
+double    OM210;
+double    BS110;
+double    BS910;
+double    AlF110;
+double    AlF310;
+double    S10p3;
+double    C10p3;
+double    AlM110_3;
+double    AlM310_3;
+double    OM211;
+double    S10p3p11;
+double    C10p3p11;
+double    OM212;
+double    S10p3p12;
+double    C10p3p12;
+double    FA112;
+double    FA312;
+double    CF212;
+double    CM212_1;
+double    CM212_2;
+double    FB112_3;
+double    FB312_3;
+double    CM212_3;
+double    FB112_10;
+double    FB312_10;
+double    CM212_10;
+double    CM212_12;
+double    FA111;
+double    FA311;
+double    CF211;
+double    CM211_1;
+double    CM211_2;
+double    FB111_3;
+double    FB311_3;
+double    CM211_3;
+double    FB111_10;
+double    FB311_10;
+double    CM211_10;
+double    CM211_11;
+double    FA110;
+double    FA310;
+double    FF110;
+double    FF310;
+double    CF210;
+double    FB110_1;
+double    FB310_1;
+double    FM110_1;
+double    FM310_1;
+double    CM210_1;
+double    FB110_2;
+double    FB310_2;
+double    FM110_2;
+double    FM310_2;
+double    CM210_2;
+double    FB110_3;
+double    FB310_3;
+double    FM110_3;
+double    FM310_3;
+double    CM210_3;
+double    CM210_10;
+double    FA19;
+double    FA39;
+double    CF29;
+double    FB19_1;
+double    FB39_1;
+double    CM29_1;
+double    FB19_2;
+double    FB39_2;
+double    CM29_2;
+double    FB19_3;
+double    FB39_3;
+double    CM29_3;
+double    FB19_7;
+double    FB39_7;
+double    CM29_7;
+double    FB19_8;
+double    FB39_8;
+double    CM29_8;
+double    CM29_9;
+double    FA18;
+double    FA38;
+double    FF18;
+double    FF38;
+double    CF28;
+double    FB18_1;
+double    FB38_1;
+double    CM28_1;
+double    FB18_2;
+double    FB38_2;
+double    CM28_2;
+double    FB18_3;
+double    FB38_3;
+double    CM28_3;
+double    FB18_7;
+double    FB38_7;
+double    CM28_7;
+double    CM28_8;
+double    FA17;
+double    FA37;
+double    FF17;
+double    FF37;
+double    CF27;
+double    CM27_1;
+double    FB17_2;
+double    FB37_2;
+double    CM27_2;
+double    CM27_3;
+double    CM27_7;
+double    FA16;
+double    FA36;
+double    CF26;
+double    FB16_1;
+double    FB36_1;
+double    CM26_1;
+double    FB16_2;
+double    FB36_2;
+double    CM26_2;
+double    FB16_3;
+double    FB36_3;
+double    CM26_3;
+double    FB16_4;
+double    FB36_4;
+double    CM26_4;
+double    FB16_5;
+double    FB36_5;
+double    CM26_5;
+double    CM26_6;
+double    FA15;
+double    FA35;
+double    FF15;
+double    FF35;
+double    CF25;
+double    FB15_1;
+double    FB35_1;
+double    CM25_1;
+double    FB15_2;
+double    FB35_2;
+double    CM25_2;
+double    FB15_3;
+double    FB35_3;
+double    CM25_3;
+double    FB15_4;
+double    FB35_4;
+double    CM25_4;
+double    CM25_5;
+double    FA14;
+double    FA34;
+double    FF14;
+double    FF34;
+double    CF24;
+double    CM24_1;
+double    FB14_2;
+double    FB34_2;
+double    CM24_2;
+double    CM24_3;
+double    CM24_4;
+double    FA13;
+double    FA33;
+double    FF13;
+double    FF33;
+double    CF23;
+double    FB13_1;
+double    FB33_1;
+double    CM23_1;
+double    FB13_2;
+double    FB33_2;
+double    CM23_2;
+double    CM23_3;
+double    FF2_13;
+double    FF2_33;
+double    FM21_13;
+double    FM22_33;
diff --git a/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.c b/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.c
new file mode 100644
index 0000000..9b52fd8
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.c
@@ -0,0 +1,316 @@
+//
+//-------------------------------------------------------------
+//
+//	ROBOTRAN - Version 6.6 (build : february 22, 2008)
+//
+//	Copyright 
+//	Universite catholique de Louvain 
+//	Departement de Mecanique 
+//	Unite de Production Mecanique et Machines 
+//	2, Place du Levant 
+//	1348 Louvain-la-Neuve 
+//	http://www.robotran.be// 
+//
+//	==> Generation Date : Fri Jan 27 19:08:05 2017
+//
+//	==> Project name : coman_short_feet_simp
+//	==> using XML input file 
+//
+//	==> Number of joints : 12
+//
+//	==> Function : F19 : External Forces
+//	==> Flops complexity : 194
+//
+//	==> Generation Time :  0.000 seconds
+//	==> Post-Processing :  0.010 seconds
+//
+//-------------------------------------------------------------
+//
+ 
+#include <math.h> 
+
+#include "mbs_data.h"
+#include "mbs_project_interface.h"
+ 
+void mbs_extforces(double **frc,double **trq,
+MbsData *s, double tsim)
+
+// double frc[3][12];
+// double trq[3][12];
+{ 
+double PxF1[4]; 
+double RxF1[4][4]; 
+double VxF1[4]; 
+double OMxF1[4]; 
+double AxF1[4]; 
+double OMPxF1[4]; 
+double *SWr1; 
+double PxF2[4]; 
+double RxF2[4][4]; 
+double VxF2[4]; 
+double OMxF2[4]; 
+double AxF2[4]; 
+double OMPxF2[4]; 
+double *SWr2; 
+ 
+#include "mbs_extforces_coman_short_feet_simp.h" 
+#define q s->q 
+#define qd s->qd 
+#define qdd s->qdd 
+ 
+ 
+
+// === begin imp_aux === 
+
+// === end imp_aux === 
+
+// ===== BEGIN task 0 ===== 
+ 
+// Sensor Kinematics 
+
+
+
+// = = Block_0_0_0_0_0_1 = = 
+ 
+// Trigonometric Variables  
+
+  C3 = cos(q[3]);
+  S3 = sin(q[3]);
+
+// = = Block_0_0_0_0_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  C4 = cos(q[4]);
+  S4 = sin(q[4]);
+  C5 = cos(q[5]);
+  S5 = sin(q[5]);
+  C6 = cos(q[6]);
+  S6 = sin(q[6]);
+
+// = = Block_0_0_0_0_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  C7 = cos(q[7]);
+  S7 = sin(q[7]);
+  C8 = cos(q[8]);
+  S8 = sin(q[8]);
+  C9 = cos(q[9]);
+  S9 = sin(q[9]);
+
+// = = Block_0_0_1_1_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4 = C3*S4+S3*C4;
+  C3p4 = C3*C4-S3*S4;
+  S3p4p5 = C3p4*S5+S3p4*C5;
+  C3p4p5 = C3p4*C5-S3p4*S5;
+  S3p4p5p6 = C3p4p5*S6+S3p4p5*C6;
+  C3p4p5p6 = C3p4p5*C6-S3p4p5*S6;
+ 
+// Sensor Kinematics 
+
+
+  OMcp0_24 = qd[3]+qd[4];
+  OPcp0_24 = qdd[3]+qdd[4];
+  RLcp0_15 = s->dpt[3][4]*S3p4;
+  RLcp0_35 = s->dpt[3][4]*C3p4;
+  OMcp0_25 = qd[5]+OMcp0_24;
+  ORcp0_15 = OMcp0_24*RLcp0_35;
+  ORcp0_35 = -OMcp0_24*RLcp0_15;
+  OPcp0_25 = qdd[5]+OPcp0_24;
+  RLcp0_16 = s->dpt[3][5]*S3p4p5;
+  RLcp0_36 = s->dpt[3][5]*C3p4p5;
+  OMcp0_26 = qd[6]+OMcp0_25;
+  ORcp0_16 = OMcp0_25*RLcp0_36;
+  ORcp0_36 = -OMcp0_25*RLcp0_16;
+  OPcp0_26 = qdd[6]+OPcp0_25;
+  RLcp0_113 = s->dpt[3][6]*S3p4p5p6;
+  RLcp0_313 = s->dpt[3][6]*C3p4p5p6;
+  ORcp0_113 = OMcp0_26*RLcp0_313;
+  ORcp0_313 = -OMcp0_26*RLcp0_113;
+  PxF1[1] = q[1]+RLcp0_113+RLcp0_15+RLcp0_16;
+  PxF1[2] = s->dpt[2][1]+s->dpt[2][4];
+  PxF1[3] = q[2]+RLcp0_313+RLcp0_35+RLcp0_36;
+  RxF1[1][1] = C3p4p5p6;
+  RxF1[1][2] = 0;
+  RxF1[1][3] = -S3p4p5p6;
+  RxF1[2][1] = 0;
+  RxF1[2][2] = (1.0);
+  RxF1[2][3] = 0;
+  RxF1[3][1] = S3p4p5p6;
+  RxF1[3][2] = 0;
+  RxF1[3][3] = C3p4p5p6;
+  VxF1[1] = qd[1]+ORcp0_113+ORcp0_15+ORcp0_16;
+  VxF1[2] = 0;
+  VxF1[3] = qd[2]+ORcp0_313+ORcp0_35+ORcp0_36;
+  OMxF1[1] = 0;
+  OMxF1[2] = OMcp0_26;
+  OMxF1[3] = 0;
+  AxF1[1] = qdd[1]+OMcp0_24*ORcp0_35+OMcp0_25*ORcp0_36+OMcp0_26*ORcp0_313+OPcp0_24*RLcp0_35+OPcp0_25*RLcp0_36+OPcp0_26*
+ RLcp0_313;
+  AxF1[2] = 0;
+  AxF1[3] = qdd[2]-OMcp0_24*ORcp0_15-OMcp0_25*ORcp0_16-OMcp0_26*ORcp0_113-OPcp0_24*RLcp0_15-OPcp0_25*RLcp0_16-OPcp0_26*
+ RLcp0_113;
+  OMPxF1[1] = 0;
+  OMPxF1[2] = OPcp0_26;
+  OMPxF1[3] = 0;
+ 
+// Sensor Forces Computation 
+
+  SWr1 = user_ExtForces(PxF1,RxF1,VxF1,OMxF1,AxF1,OMPxF1,s,tsim,1);
+ 
+// Sensor Dynamics : Forces projection on body-fixed frames 
+
+  xfrc11 = SWr1[1]*C3p4p5p6-SWr1[3]*S3p4p5p6;
+  xfrc21 = SWr1[2];
+  xfrc31 = SWr1[1]*S3p4p5p6+SWr1[3]*C3p4p5p6;
+  frc[1][6] = s->frc[1][6]+xfrc11;
+  frc[2][6] = s->frc[2][6]+SWr1[2];
+  frc[3][6] = s->frc[3][6]+xfrc31;
+  xtrq11 = SWr1[4]*C3p4p5p6-SWr1[6]*S3p4p5p6;
+  xtrq21 = SWr1[5];
+  xtrq31 = SWr1[4]*S3p4p5p6+SWr1[6]*C3p4p5p6;
+  trq[1][6] = s->trq[1][6]+xtrq11-xfrc21*(SWr1[9]-s->l[3][6])+xfrc31*SWr1[8];
+  trq[2][6] = s->trq[2][6]+xtrq21+xfrc11*(SWr1[9]-s->l[3][6])-xfrc31*(SWr1[7]-s->l[1][6]);
+  trq[3][6] = s->trq[3][6]+xtrq31-xfrc11*SWr1[8]+xfrc21*(SWr1[7]-s->l[1][6]);
+
+// = = Block_0_0_1_2_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7 = C3*S7+S3*C7;
+  C3p7 = C3*C7-S3*S7;
+  S3p7p8 = C3p7*S8+S3p7*C8;
+  C3p7p8 = C3p7*C8-S3p7*S8;
+  S3p7p8p9 = C3p7p8*S9+S3p7p8*C9;
+  C3p7p8p9 = C3p7p8*C9-S3p7p8*S9;
+ 
+// Sensor Kinematics 
+
+
+  OMcp1_27 = qd[3]+qd[7];
+  OPcp1_27 = qdd[3]+qdd[7];
+  RLcp1_18 = s->dpt[3][7]*S3p7;
+  RLcp1_38 = s->dpt[3][7]*C3p7;
+  OMcp1_28 = qd[8]+OMcp1_27;
+  ORcp1_18 = OMcp1_27*RLcp1_38;
+  ORcp1_38 = -OMcp1_27*RLcp1_18;
+  OPcp1_28 = qdd[8]+OPcp1_27;
+  RLcp1_19 = s->dpt[3][8]*S3p7p8;
+  RLcp1_39 = s->dpt[3][8]*C3p7p8;
+  OMcp1_29 = qd[9]+OMcp1_28;
+  ORcp1_19 = OMcp1_28*RLcp1_39;
+  ORcp1_39 = -OMcp1_28*RLcp1_19;
+  OPcp1_29 = qdd[9]+OPcp1_28;
+  RLcp1_114 = s->dpt[3][9]*S3p7p8p9;
+  RLcp1_314 = s->dpt[3][9]*C3p7p8p9;
+  ORcp1_114 = OMcp1_29*RLcp1_314;
+  ORcp1_314 = -OMcp1_29*RLcp1_114;
+  PxF2[1] = q[1]+RLcp1_114+RLcp1_18+RLcp1_19;
+  PxF2[2] = s->dpt[2][2]+s->dpt[2][7];
+  PxF2[3] = q[2]+RLcp1_314+RLcp1_38+RLcp1_39;
+  RxF2[1][1] = C3p7p8p9;
+  RxF2[1][2] = 0;
+  RxF2[1][3] = -S3p7p8p9;
+  RxF2[2][1] = 0;
+  RxF2[2][2] = (1.0);
+  RxF2[2][3] = 0;
+  RxF2[3][1] = S3p7p8p9;
+  RxF2[3][2] = 0;
+  RxF2[3][3] = C3p7p8p9;
+  VxF2[1] = qd[1]+ORcp1_114+ORcp1_18+ORcp1_19;
+  VxF2[2] = 0;
+  VxF2[3] = qd[2]+ORcp1_314+ORcp1_38+ORcp1_39;
+  OMxF2[1] = 0;
+  OMxF2[2] = OMcp1_29;
+  OMxF2[3] = 0;
+  AxF2[1] = qdd[1]+OMcp1_27*ORcp1_38+OMcp1_28*ORcp1_39+OMcp1_29*ORcp1_314+OPcp1_27*RLcp1_38+OPcp1_28*RLcp1_39+OPcp1_29*
+ RLcp1_314;
+  AxF2[2] = 0;
+  AxF2[3] = qdd[2]-OMcp1_27*ORcp1_18-OMcp1_28*ORcp1_19-OMcp1_29*ORcp1_114-OPcp1_27*RLcp1_18-OPcp1_28*RLcp1_19-OPcp1_29*
+ RLcp1_114;
+  OMPxF2[1] = 0;
+  OMPxF2[2] = OPcp1_29;
+  OMPxF2[3] = 0;
+ 
+// Sensor Forces Computation 
+
+  SWr2 = user_ExtForces(PxF2,RxF2,VxF2,OMxF2,AxF2,OMPxF2,s,tsim,2);
+ 
+// Sensor Dynamics : Forces projection on body-fixed frames 
+
+  xfrc12 = SWr2[1]*C3p7p8p9-SWr2[3]*S3p7p8p9;
+  xfrc22 = SWr2[2];
+  xfrc32 = SWr2[1]*S3p7p8p9+SWr2[3]*C3p7p8p9;
+  frc[1][9] = s->frc[1][9]+xfrc12;
+  frc[2][9] = s->frc[2][9]+SWr2[2];
+  frc[3][9] = s->frc[3][9]+xfrc32;
+  xtrq12 = SWr2[4]*C3p7p8p9-SWr2[6]*S3p7p8p9;
+  xtrq22 = SWr2[5];
+  xtrq32 = SWr2[4]*S3p7p8p9+SWr2[6]*C3p7p8p9;
+  trq[1][9] = s->trq[1][9]+xtrq12-xfrc22*(SWr2[9]-s->l[3][9])+xfrc32*SWr2[8];
+  trq[2][9] = s->trq[2][9]+xtrq22+xfrc12*(SWr2[9]-s->l[3][9])-xfrc32*(SWr2[7]-s->l[1][9]);
+  trq[3][9] = s->trq[3][9]+xtrq32-xfrc12*SWr2[8]+xfrc22*(SWr2[7]-s->l[1][9]);
+
+// = = Block_0_0_1_2_1_0 = = 
+ 
+// Symbolic Outputs  
+
+  frc[1][3] = s->frc[1][3];
+  frc[2][3] = s->frc[2][3];
+  frc[3][3] = s->frc[3][3];
+  frc[1][4] = s->frc[1][4];
+  frc[2][4] = s->frc[2][4];
+  frc[3][4] = s->frc[3][4];
+  frc[1][5] = s->frc[1][5];
+  frc[2][5] = s->frc[2][5];
+  frc[3][5] = s->frc[3][5];
+  frc[1][7] = s->frc[1][7];
+  frc[2][7] = s->frc[2][7];
+  frc[3][7] = s->frc[3][7];
+  frc[1][8] = s->frc[1][8];
+  frc[2][8] = s->frc[2][8];
+  frc[3][8] = s->frc[3][8];
+  frc[1][10] = s->frc[1][10];
+  frc[2][10] = s->frc[2][10];
+  frc[3][10] = s->frc[3][10];
+  frc[1][11] = s->frc[1][11];
+  frc[2][11] = s->frc[2][11];
+  frc[3][11] = s->frc[3][11];
+  frc[1][12] = s->frc[1][12];
+  frc[2][12] = s->frc[2][12];
+  frc[3][12] = s->frc[3][12];
+  trq[1][3] = s->trq[1][3];
+  trq[2][3] = s->trq[2][3];
+  trq[3][3] = s->trq[3][3];
+  trq[1][4] = s->trq[1][4];
+  trq[2][4] = s->trq[2][4];
+  trq[3][4] = s->trq[3][4];
+  trq[1][5] = s->trq[1][5];
+  trq[2][5] = s->trq[2][5];
+  trq[3][5] = s->trq[3][5];
+  trq[1][7] = s->trq[1][7];
+  trq[2][7] = s->trq[2][7];
+  trq[3][7] = s->trq[3][7];
+  trq[1][8] = s->trq[1][8];
+  trq[2][8] = s->trq[2][8];
+  trq[3][8] = s->trq[3][8];
+  trq[1][10] = s->trq[1][10];
+  trq[2][10] = s->trq[2][10];
+  trq[3][10] = s->trq[3][10];
+  trq[1][11] = s->trq[1][11];
+  trq[2][11] = s->trq[2][11];
+  trq[3][11] = s->trq[3][11];
+  trq[1][12] = s->trq[1][12];
+  trq[2][12] = s->trq[2][12];
+  trq[3][12] = s->trq[3][12];
+
+// ====== END Task 0 ====== 
+
+
+}
+ 
+
diff --git a/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.h b/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.h
new file mode 100644
index 0000000..97ffa21
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_extforces_coman_short_feet_simp.h
@@ -0,0 +1,74 @@
+double    C3;
+double    S3;
+double    C4;
+double    S4;
+double    C5;
+double    S5;
+double    C6;
+double    S6;
+double    C7;
+double    S7;
+double    C8;
+double    S8;
+double    C9;
+double    S9;
+double    S3p4;
+double    C3p4;
+double    S3p4p5;
+double    C3p4p5;
+double    S3p4p5p6;
+double    C3p4p5p6;
+double    OMcp0_24;
+double    OPcp0_24;
+double    RLcp0_15;
+double    RLcp0_35;
+double    OMcp0_25;
+double    ORcp0_15;
+double    ORcp0_35;
+double    OPcp0_25;
+double    RLcp0_16;
+double    RLcp0_36;
+double    OMcp0_26;
+double    ORcp0_16;
+double    ORcp0_36;
+double    OPcp0_26;
+double    RLcp0_113;
+double    RLcp0_313;
+double    ORcp0_113;
+double    ORcp0_313;
+double    xfrc11;
+double    xfrc21;
+double    xfrc31;
+double    xtrq11;
+double    xtrq21;
+double    xtrq31;
+double    S3p7;
+double    C3p7;
+double    S3p7p8;
+double    C3p7p8;
+double    S3p7p8p9;
+double    C3p7p8p9;
+double    OMcp1_27;
+double    OPcp1_27;
+double    RLcp1_18;
+double    RLcp1_38;
+double    OMcp1_28;
+double    ORcp1_18;
+double    ORcp1_38;
+double    OPcp1_28;
+double    RLcp1_19;
+double    RLcp1_39;
+double    OMcp1_29;
+double    ORcp1_19;
+double    ORcp1_39;
+double    OPcp1_29;
+double    RLcp1_114;
+double    RLcp1_314;
+double    ORcp1_114;
+double    ORcp1_314;
+double    xfrc12;
+double    xfrc22;
+double    xfrc32;
+double    xtrq12;
+double    xtrq22;
+double    xtrq32;
diff --git a/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.c b/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.c
new file mode 100644
index 0000000..3739f6b
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.c
@@ -0,0 +1,664 @@
+//
+//-------------------------------------------------------------
+//
+//	ROBOTRAN - Version 6.6 (build : february 22, 2008)
+//
+//	Copyright 
+//	Universite catholique de Louvain 
+//	Departement de Mecanique 
+//	Unite de Production Mecanique et Machines 
+//	2, Place du Levant 
+//	1348 Louvain-la-Neuve 
+//	http://www.robotran.be// 
+//
+//	==> Generation Date : Fri Jan 27 19:08:05 2017
+//
+//	==> Project name : coman_short_feet_simp
+//	==> using XML input file 
+//
+//	==> Number of joints : 12
+//
+//	==> Function : F 6 : Sensors Kinematical Informations (sens) 
+//	==> Flops complexity : 324
+//
+//	==> Generation Time :  0.000 seconds
+//	==> Post-Processing :  0.010 seconds
+//
+//-------------------------------------------------------------
+//
+ 
+#include <math.h> 
+
+#include "mbs_data.h"
+#include "mbs_project_interface.h"
+#include "mbs_sensor.h"
+ 
+void  mbs_gensensor(MbsSensor *sens, 
+              MbsData *s,
+              int isens)
+{ 
+ 
+#include "mbs_gensensor_coman_short_feet_simp.h" 
+#define q s->q 
+#define qd s->qd 
+#define qdd s->qdd 
+ 
+ 
+
+// === begin imp_aux === 
+
+// === end imp_aux === 
+
+// ===== BEGIN task 0 ===== 
+ 
+// Sensor Kinematics 
+
+
+
+// = = Block_0_0_0_0_0_1 = = 
+ 
+// Trigonometric Variables  
+
+  C3 = cos(q[3]);
+  S3 = sin(q[3]);
+
+// = = Block_0_0_0_0_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  C4 = cos(q[4]);
+  S4 = sin(q[4]);
+  C5 = cos(q[5]);
+  S5 = sin(q[5]);
+  C6 = cos(q[6]);
+  S6 = sin(q[6]);
+
+// = = Block_0_0_0_0_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  C7 = cos(q[7]);
+  S7 = sin(q[7]);
+  C8 = cos(q[8]);
+  S8 = sin(q[8]);
+  C9 = cos(q[9]);
+  S9 = sin(q[9]);
+
+// = = Block_0_0_0_0_0_4 = = 
+ 
+// Trigonometric Variables  
+
+  C10 = cos(q[10]);
+  S10 = sin(q[10]);
+
+// = = Block_0_0_0_0_0_5 = = 
+ 
+// Trigonometric Variables  
+
+  C11 = cos(q[11]);
+  S11 = sin(q[11]);
+
+// = = Block_0_0_0_0_0_6 = = 
+ 
+// Trigonometric Variables  
+
+  C12 = cos(q[12]);
+  S12 = sin(q[12]);
+
+// = = Block_0_0_0_4_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4 = C3*S4+S3*C4;
+  C3p4 = C3*C4-S3*S4;
+
+// = = Block_0_0_0_5_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4p5 = C3p4*S5+S3p4*C5;
+  C3p4p5 = C3p4*C5-S3p4*S5;
+
+// = = Block_0_0_0_6_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4p5p6 = C3p4p5*S6+S3p4p5*C6;
+  C3p4p5p6 = C3p4p5*C6-S3p4p5*S6;
+
+// = = Block_0_0_0_7_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7 = C3*S7+S3*C7;
+  C3p7 = C3*C7-S3*S7;
+
+// = = Block_0_0_0_8_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7p8 = C3p7*S8+S3p7*C8;
+  C3p7p8 = C3p7*C8-S3p7*S8;
+
+// = = Block_0_0_0_9_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7p8p9 = C3p7p8*S9+S3p7p8*C9;
+  C3p7p8p9 = C3p7p8*C9-S3p7p8*S9;
+
+// = = Block_0_0_0_10_0_4 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3 = C10*S3+S10*C3;
+  C10p3 = C10*C3-S10*S3;
+
+// = = Block_0_0_0_11_0_5 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3p11 = C10p3*S11+S10p3*C11;
+  C10p3p11 = C10p3*C11-S10p3*S11;
+
+// = = Block_0_0_0_12_0_6 = = 
+ 
+// Trigonometric Variables  
+
+  S10p3p12 = C10p3*S12+S10p3*C12;
+  C10p3p12 = C10p3*C12-S10p3*S12;
+
+// ====== END Task 0 ====== 
+
+// ===== BEGIN task 1 ===== 
+ 
+switch(isens)
+{
+ 
+// 
+break;
+case 1:
+ 
+
+
+// = = Block_1_0_0_1_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = q[1];
+    sens->R[1][1] = (1.0);
+    sens->R[2][2] = (1.0);
+    sens->R[3][3] = (1.0);
+    sens->V[1] = qd[1];
+    sens->A[1] = qdd[1];
+ 
+// 
+break;
+case 2:
+ 
+
+
+// = = Block_1_0_0_2_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = q[1];
+    sens->P[3] = q[2];
+    sens->R[1][1] = (1.0);
+    sens->R[2][2] = (1.0);
+    sens->R[3][3] = (1.0);
+    sens->V[1] = qd[1];
+    sens->V[3] = qd[2];
+    sens->A[1] = qdd[1];
+    sens->A[3] = qdd[2];
+ 
+// 
+break;
+case 3:
+ 
+
+
+// = = Block_1_0_0_3_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = q[1];
+    sens->P[3] = q[2];
+    sens->R[1][1] = C3;
+    sens->R[1][3] = -S3;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3;
+    sens->R[3][3] = C3;
+    sens->V[1] = qd[1];
+    sens->V[3] = qd[2];
+    sens->OM[2] = qd[3];
+    sens->A[1] = qdd[1];
+    sens->A[3] = qdd[2];
+    sens->OMP[2] = qdd[3];
+ 
+// 
+break;
+case 4:
+ 
+
+
+// = = Block_1_0_0_4_0_2 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp3_24 = qd[3]+qd[4];
+    OPcp3_24 = qdd[3]+qdd[4];
+
+// = = Block_1_0_0_4_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = q[1];
+    sens->P[2] = s->dpt[2][1];
+    sens->P[3] = q[2];
+    sens->R[1][1] = C3p4;
+    sens->R[1][3] = -S3p4;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p4;
+    sens->R[3][3] = C3p4;
+    sens->V[1] = qd[1];
+    sens->V[3] = qd[2];
+    sens->OM[2] = OMcp3_24;
+    sens->A[1] = qdd[1];
+    sens->A[3] = qdd[2];
+    sens->OMP[2] = OPcp3_24;
+ 
+// 
+break;
+case 5:
+ 
+
+
+// = = Block_1_0_0_5_0_2 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp4_24 = qd[3]+qd[4];
+    OPcp4_24 = qdd[3]+qdd[4];
+    RLcp4_15 = s->dpt[3][4]*S3p4;
+    RLcp4_35 = s->dpt[3][4]*C3p4;
+    POcp4_15 = RLcp4_15+q[1];
+    POcp4_25 = s->dpt[2][1]+s->dpt[2][4];
+    POcp4_35 = RLcp4_35+q[2];
+    OMcp4_25 = OMcp4_24+qd[5];
+    ORcp4_15 = OMcp4_24*RLcp4_35;
+    ORcp4_35 = -OMcp4_24*RLcp4_15;
+    VIcp4_15 = ORcp4_15+qd[1];
+    VIcp4_35 = ORcp4_35+qd[2];
+    OPcp4_25 = OPcp4_24+qdd[5];
+    ACcp4_15 = qdd[1]+OMcp4_24*ORcp4_35+OPcp4_24*RLcp4_35;
+    ACcp4_35 = qdd[2]-OMcp4_24*ORcp4_15-OPcp4_24*RLcp4_15;
+
+// = = Block_1_0_0_5_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp4_15;
+    sens->P[2] = POcp4_25;
+    sens->P[3] = POcp4_35;
+    sens->R[1][1] = C3p4p5;
+    sens->R[1][3] = -S3p4p5;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p4p5;
+    sens->R[3][3] = C3p4p5;
+    sens->V[1] = VIcp4_15;
+    sens->V[3] = VIcp4_35;
+    sens->OM[2] = OMcp4_25;
+    sens->A[1] = ACcp4_15;
+    sens->A[3] = ACcp4_35;
+    sens->OMP[2] = OPcp4_25;
+ 
+// 
+break;
+case 6:
+ 
+
+
+// = = Block_1_0_0_6_0_2 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp5_24 = qd[3]+qd[4];
+    OPcp5_24 = qdd[3]+qdd[4];
+    RLcp5_15 = s->dpt[3][4]*S3p4;
+    RLcp5_35 = s->dpt[3][4]*C3p4;
+    POcp5_25 = s->dpt[2][1]+s->dpt[2][4];
+    OMcp5_25 = OMcp5_24+qd[5];
+    ORcp5_15 = OMcp5_24*RLcp5_35;
+    ORcp5_35 = -OMcp5_24*RLcp5_15;
+    OPcp5_25 = OPcp5_24+qdd[5];
+    RLcp5_16 = s->dpt[3][5]*S3p4p5;
+    RLcp5_36 = s->dpt[3][5]*C3p4p5;
+    POcp5_16 = RLcp5_15+RLcp5_16+q[1];
+    POcp5_36 = RLcp5_35+RLcp5_36+q[2];
+    OMcp5_26 = OMcp5_25+qd[6];
+    ORcp5_16 = OMcp5_25*RLcp5_36;
+    ORcp5_36 = -OMcp5_25*RLcp5_16;
+    VIcp5_16 = ORcp5_15+ORcp5_16+qd[1];
+    VIcp5_36 = ORcp5_35+ORcp5_36+qd[2];
+    OPcp5_26 = OPcp5_25+qdd[6];
+    ACcp5_16 = qdd[1]+OMcp5_24*ORcp5_35+OMcp5_25*ORcp5_36+OPcp5_24*RLcp5_35+OPcp5_25*RLcp5_36;
+    ACcp5_36 = qdd[2]-OMcp5_24*ORcp5_15-OMcp5_25*ORcp5_16-OPcp5_24*RLcp5_15-OPcp5_25*RLcp5_16;
+
+// = = Block_1_0_0_6_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp5_16;
+    sens->P[2] = POcp5_25;
+    sens->P[3] = POcp5_36;
+    sens->R[1][1] = C3p4p5p6;
+    sens->R[1][3] = -S3p4p5p6;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p4p5p6;
+    sens->R[3][3] = C3p4p5p6;
+    sens->V[1] = VIcp5_16;
+    sens->V[3] = VIcp5_36;
+    sens->OM[2] = OMcp5_26;
+    sens->A[1] = ACcp5_16;
+    sens->A[3] = ACcp5_36;
+    sens->OMP[2] = OPcp5_26;
+ 
+// 
+break;
+case 7:
+ 
+
+
+// = = Block_1_0_0_7_0_3 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp6_27 = qd[3]+qd[7];
+    OPcp6_27 = qdd[3]+qdd[7];
+
+// = = Block_1_0_0_7_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = q[1];
+    sens->P[2] = s->dpt[2][2];
+    sens->P[3] = q[2];
+    sens->R[1][1] = C3p7;
+    sens->R[1][3] = -S3p7;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p7;
+    sens->R[3][3] = C3p7;
+    sens->V[1] = qd[1];
+    sens->V[3] = qd[2];
+    sens->OM[2] = OMcp6_27;
+    sens->A[1] = qdd[1];
+    sens->A[3] = qdd[2];
+    sens->OMP[2] = OPcp6_27;
+ 
+// 
+break;
+case 8:
+ 
+
+
+// = = Block_1_0_0_8_0_3 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp7_27 = qd[3]+qd[7];
+    OPcp7_27 = qdd[3]+qdd[7];
+    RLcp7_18 = s->dpt[3][7]*S3p7;
+    RLcp7_38 = s->dpt[3][7]*C3p7;
+    POcp7_18 = RLcp7_18+q[1];
+    POcp7_28 = s->dpt[2][2]+s->dpt[2][7];
+    POcp7_38 = RLcp7_38+q[2];
+    OMcp7_28 = OMcp7_27+qd[8];
+    ORcp7_18 = OMcp7_27*RLcp7_38;
+    ORcp7_38 = -OMcp7_27*RLcp7_18;
+    VIcp7_18 = ORcp7_18+qd[1];
+    VIcp7_38 = ORcp7_38+qd[2];
+    OPcp7_28 = OPcp7_27+qdd[8];
+    ACcp7_18 = qdd[1]+OMcp7_27*ORcp7_38+OPcp7_27*RLcp7_38;
+    ACcp7_38 = qdd[2]-OMcp7_27*ORcp7_18-OPcp7_27*RLcp7_18;
+
+// = = Block_1_0_0_8_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp7_18;
+    sens->P[2] = POcp7_28;
+    sens->P[3] = POcp7_38;
+    sens->R[1][1] = C3p7p8;
+    sens->R[1][3] = -S3p7p8;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p7p8;
+    sens->R[3][3] = C3p7p8;
+    sens->V[1] = VIcp7_18;
+    sens->V[3] = VIcp7_38;
+    sens->OM[2] = OMcp7_28;
+    sens->A[1] = ACcp7_18;
+    sens->A[3] = ACcp7_38;
+    sens->OMP[2] = OPcp7_28;
+ 
+// 
+break;
+case 9:
+ 
+
+
+// = = Block_1_0_0_9_0_3 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp8_27 = qd[3]+qd[7];
+    OPcp8_27 = qdd[3]+qdd[7];
+    RLcp8_18 = s->dpt[3][7]*S3p7;
+    RLcp8_38 = s->dpt[3][7]*C3p7;
+    POcp8_28 = s->dpt[2][2]+s->dpt[2][7];
+    OMcp8_28 = OMcp8_27+qd[8];
+    ORcp8_18 = OMcp8_27*RLcp8_38;
+    ORcp8_38 = -OMcp8_27*RLcp8_18;
+    OPcp8_28 = OPcp8_27+qdd[8];
+    RLcp8_19 = s->dpt[3][8]*S3p7p8;
+    RLcp8_39 = s->dpt[3][8]*C3p7p8;
+    POcp8_19 = RLcp8_18+RLcp8_19+q[1];
+    POcp8_39 = RLcp8_38+RLcp8_39+q[2];
+    OMcp8_29 = OMcp8_28+qd[9];
+    ORcp8_19 = OMcp8_28*RLcp8_39;
+    ORcp8_39 = -OMcp8_28*RLcp8_19;
+    VIcp8_19 = ORcp8_18+ORcp8_19+qd[1];
+    VIcp8_39 = ORcp8_38+ORcp8_39+qd[2];
+    OPcp8_29 = OPcp8_28+qdd[9];
+    ACcp8_19 = qdd[1]+OMcp8_27*ORcp8_38+OMcp8_28*ORcp8_39+OPcp8_27*RLcp8_38+OPcp8_28*RLcp8_39;
+    ACcp8_39 = qdd[2]-OMcp8_27*ORcp8_18-OMcp8_28*ORcp8_19-OPcp8_27*RLcp8_18-OPcp8_28*RLcp8_19;
+
+// = = Block_1_0_0_9_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp8_19;
+    sens->P[2] = POcp8_28;
+    sens->P[3] = POcp8_39;
+    sens->R[1][1] = C3p7p8p9;
+    sens->R[1][3] = -S3p7p8p9;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p7p8p9;
+    sens->R[3][3] = C3p7p8p9;
+    sens->V[1] = VIcp8_19;
+    sens->V[3] = VIcp8_39;
+    sens->OM[2] = OMcp8_29;
+    sens->A[1] = ACcp8_19;
+    sens->A[3] = ACcp8_39;
+    sens->OMP[2] = OPcp8_29;
+ 
+// 
+break;
+case 10:
+ 
+
+
+// = = Block_1_0_0_10_0_4 = = 
+ 
+// Sensor Kinematics 
+
+
+    RLcp9_110 = s->dpt[1][3]*C3+s->dpt[3][3]*S3;
+    RLcp9_310 = -(s->dpt[1][3]*S3-s->dpt[3][3]*C3);
+    POcp9_110 = RLcp9_110+q[1];
+    POcp9_310 = RLcp9_310+q[2];
+    OMcp9_210 = qd[10]+qd[3];
+    ORcp9_110 = RLcp9_310*qd[3];
+    ORcp9_310 = -RLcp9_110*qd[3];
+    VIcp9_110 = ORcp9_110+qd[1];
+    VIcp9_310 = ORcp9_310+qd[2];
+    OPcp9_210 = qdd[10]+qdd[3];
+    ACcp9_110 = qdd[1]+ORcp9_310*qd[3]+RLcp9_310*qdd[3];
+    ACcp9_310 = qdd[2]-ORcp9_110*qd[3]-RLcp9_110*qdd[3];
+
+// = = Block_1_0_0_10_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp9_110;
+    sens->P[3] = POcp9_310;
+    sens->R[1][1] = C10p3;
+    sens->R[1][3] = -S10p3;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S10p3;
+    sens->R[3][3] = C10p3;
+    sens->V[1] = VIcp9_110;
+    sens->V[3] = VIcp9_310;
+    sens->OM[2] = OMcp9_210;
+    sens->A[1] = ACcp9_110;
+    sens->A[3] = ACcp9_310;
+    sens->OMP[2] = OPcp9_210;
+ 
+// 
+break;
+case 11:
+ 
+
+
+// = = Block_1_0_0_11_0_4 = = 
+ 
+// Sensor Kinematics 
+
+
+    RLcp10_110 = s->dpt[1][3]*C3+s->dpt[3][3]*S3;
+    RLcp10_310 = -(s->dpt[1][3]*S3-s->dpt[3][3]*C3);
+    OMcp10_210 = qd[10]+qd[3];
+    ORcp10_110 = RLcp10_310*qd[3];
+    ORcp10_310 = -RLcp10_110*qd[3];
+    OPcp10_210 = qdd[10]+qdd[3];
+
+// = = Block_1_0_0_11_0_5 = = 
+ 
+// Sensor Kinematics 
+
+
+    RLcp10_111 = s->dpt[1][10]*C10p3+s->dpt[3][10]*S10p3;
+    RLcp10_311 = -(s->dpt[1][10]*S10p3-s->dpt[3][10]*C10p3);
+    POcp10_111 = RLcp10_110+RLcp10_111+q[1];
+    POcp10_311 = RLcp10_310+RLcp10_311+q[2];
+    OMcp10_211 = OMcp10_210+qd[11];
+    ORcp10_111 = OMcp10_210*RLcp10_311;
+    ORcp10_311 = -OMcp10_210*RLcp10_111;
+    VIcp10_111 = ORcp10_110+ORcp10_111+qd[1];
+    VIcp10_311 = ORcp10_310+ORcp10_311+qd[2];
+    OPcp10_211 = OPcp10_210+qdd[11];
+    ACcp10_111 = qdd[1]+OMcp10_210*ORcp10_311+OPcp10_210*RLcp10_311+ORcp10_310*qd[3]+RLcp10_310*qdd[3];
+    ACcp10_311 = qdd[2]-OMcp10_210*ORcp10_111-OPcp10_210*RLcp10_111-ORcp10_110*qd[3]-RLcp10_110*qdd[3];
+
+// = = Block_1_0_0_11_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp10_111;
+    sens->P[2] = s->dpt[2][10];
+    sens->P[3] = POcp10_311;
+    sens->R[1][1] = C10p3p11;
+    sens->R[1][3] = -S10p3p11;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S10p3p11;
+    sens->R[3][3] = C10p3p11;
+    sens->V[1] = VIcp10_111;
+    sens->V[3] = VIcp10_311;
+    sens->OM[2] = OMcp10_211;
+    sens->A[1] = ACcp10_111;
+    sens->A[3] = ACcp10_311;
+    sens->OMP[2] = OPcp10_211;
+ 
+// 
+break;
+case 12:
+ 
+
+
+// = = Block_1_0_0_12_0_4 = = 
+ 
+// Sensor Kinematics 
+
+
+    RLcp11_110 = s->dpt[1][3]*C3+s->dpt[3][3]*S3;
+    RLcp11_310 = -(s->dpt[1][3]*S3-s->dpt[3][3]*C3);
+    OMcp11_210 = qd[10]+qd[3];
+    ORcp11_110 = RLcp11_310*qd[3];
+    ORcp11_310 = -RLcp11_110*qd[3];
+    OPcp11_210 = qdd[10]+qdd[3];
+
+// = = Block_1_0_0_12_0_6 = = 
+ 
+// Sensor Kinematics 
+
+
+    RLcp11_112 = s->dpt[1][11]*C10p3+s->dpt[3][11]*S10p3;
+    RLcp11_312 = -(s->dpt[1][11]*S10p3-s->dpt[3][11]*C10p3);
+    POcp11_112 = RLcp11_110+RLcp11_112+q[1];
+    POcp11_312 = RLcp11_310+RLcp11_312+q[2];
+    OMcp11_212 = OMcp11_210+qd[12];
+    ORcp11_112 = OMcp11_210*RLcp11_312;
+    ORcp11_312 = -OMcp11_210*RLcp11_112;
+    VIcp11_112 = ORcp11_110+ORcp11_112+qd[1];
+    VIcp11_312 = ORcp11_310+ORcp11_312+qd[2];
+    OPcp11_212 = OPcp11_210+qdd[12];
+    ACcp11_112 = qdd[1]+OMcp11_210*ORcp11_312+OPcp11_210*RLcp11_312+ORcp11_310*qd[3]+RLcp11_310*qdd[3];
+    ACcp11_312 = qdd[2]-OMcp11_210*ORcp11_112-OPcp11_210*RLcp11_112-ORcp11_110*qd[3]-RLcp11_110*qdd[3];
+
+// = = Block_1_0_0_12_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp11_112;
+    sens->P[2] = s->dpt[2][11];
+    sens->P[3] = POcp11_312;
+    sens->R[1][1] = C10p3p12;
+    sens->R[1][3] = -S10p3p12;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S10p3p12;
+    sens->R[3][3] = C10p3p12;
+    sens->V[1] = VIcp11_112;
+    sens->V[3] = VIcp11_312;
+    sens->OM[2] = OMcp11_212;
+    sens->A[1] = ACcp11_112;
+    sens->A[3] = ACcp11_312;
+    sens->OMP[2] = OPcp11_212;
+
+break;
+default:
+break;
+}
+
+
+// ====== END Task 1 ====== 
+
+
+}
+ 
+
diff --git a/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.h b/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.h
new file mode 100644
index 0000000..731f4e0
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_gensensor_coman_short_feet_simp.h
@@ -0,0 +1,162 @@
+double    C3;
+double    S3;
+double    C4;
+double    S4;
+double    C5;
+double    S5;
+double    C6;
+double    S6;
+double    C7;
+double    S7;
+double    C8;
+double    S8;
+double    C9;
+double    S9;
+double    C10;
+double    S10;
+double    C11;
+double    S11;
+double    C12;
+double    S12;
+double    S3p4;
+double    C3p4;
+double    OMcp3_24;
+double    OPcp3_24;
+double    S3p4p5;
+double    C3p4p5;
+double    OMcp4_24;
+double    OPcp4_24;
+double    RLcp4_15;
+double    RLcp4_35;
+double    POcp4_15;
+double    POcp4_25;
+double    POcp4_35;
+double    OMcp4_25;
+double    ORcp4_15;
+double    ORcp4_35;
+double    VIcp4_15;
+double    VIcp4_35;
+double    OPcp4_25;
+double    ACcp4_15;
+double    ACcp4_35;
+double    S3p4p5p6;
+double    C3p4p5p6;
+double    OMcp5_24;
+double    OPcp5_24;
+double    RLcp5_15;
+double    RLcp5_35;
+double    POcp5_25;
+double    OMcp5_25;
+double    ORcp5_15;
+double    ORcp5_35;
+double    OPcp5_25;
+double    RLcp5_16;
+double    RLcp5_36;
+double    POcp5_16;
+double    POcp5_36;
+double    OMcp5_26;
+double    ORcp5_16;
+double    ORcp5_36;
+double    VIcp5_16;
+double    VIcp5_36;
+double    OPcp5_26;
+double    ACcp5_16;
+double    ACcp5_36;
+double    S3p7;
+double    C3p7;
+double    OMcp6_27;
+double    OPcp6_27;
+double    S3p7p8;
+double    C3p7p8;
+double    OMcp7_27;
+double    OPcp7_27;
+double    RLcp7_18;
+double    RLcp7_38;
+double    POcp7_18;
+double    POcp7_28;
+double    POcp7_38;
+double    OMcp7_28;
+double    ORcp7_18;
+double    ORcp7_38;
+double    VIcp7_18;
+double    VIcp7_38;
+double    OPcp7_28;
+double    ACcp7_18;
+double    ACcp7_38;
+double    S3p7p8p9;
+double    C3p7p8p9;
+double    OMcp8_27;
+double    OPcp8_27;
+double    RLcp8_18;
+double    RLcp8_38;
+double    POcp8_28;
+double    OMcp8_28;
+double    ORcp8_18;
+double    ORcp8_38;
+double    OPcp8_28;
+double    RLcp8_19;
+double    RLcp8_39;
+double    POcp8_19;
+double    POcp8_39;
+double    OMcp8_29;
+double    ORcp8_19;
+double    ORcp8_39;
+double    VIcp8_19;
+double    VIcp8_39;
+double    OPcp8_29;
+double    ACcp8_19;
+double    ACcp8_39;
+double    S10p3;
+double    C10p3;
+double    RLcp9_110;
+double    RLcp9_310;
+double    POcp9_110;
+double    POcp9_310;
+double    OMcp9_210;
+double    ORcp9_110;
+double    ORcp9_310;
+double    VIcp9_110;
+double    VIcp9_310;
+double    OPcp9_210;
+double    ACcp9_110;
+double    ACcp9_310;
+double    S10p3p11;
+double    C10p3p11;
+double    RLcp10_110;
+double    RLcp10_310;
+double    OMcp10_210;
+double    ORcp10_110;
+double    ORcp10_310;
+double    OPcp10_210;
+double    RLcp10_111;
+double    RLcp10_311;
+double    POcp10_111;
+double    POcp10_311;
+double    OMcp10_211;
+double    ORcp10_111;
+double    ORcp10_311;
+double    VIcp10_111;
+double    VIcp10_311;
+double    OPcp10_211;
+double    ACcp10_111;
+double    ACcp10_311;
+double    S10p3p12;
+double    C10p3p12;
+double    RLcp11_110;
+double    RLcp11_310;
+double    OMcp11_210;
+double    ORcp11_110;
+double    ORcp11_310;
+double    OPcp11_210;
+double    RLcp11_112;
+double    RLcp11_312;
+double    POcp11_112;
+double    POcp11_312;
+double    OMcp11_212;
+double    ORcp11_112;
+double    ORcp11_312;
+double    VIcp11_112;
+double    VIcp11_312;
+double    OPcp11_212;
+double    ACcp11_112;
+double    ACcp11_312;
diff --git a/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.c b/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.c
new file mode 100644
index 0000000..e49fe8f
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.c
@@ -0,0 +1,211 @@
+//
+//-------------------------------------------------------------
+//
+//	ROBOTRAN - Version 6.6 (build : february 22, 2008)
+//
+//	Copyright 
+//	Universite catholique de Louvain 
+//	Departement de Mecanique 
+//	Unite de Production Mecanique et Machines 
+//	2, Place du Levant 
+//	1348 Louvain-la-Neuve 
+//	http://www.robotran.be// 
+//
+//	==> Generation Date : Fri Jan 27 19:08:04 2017
+//
+//	==> Project name : coman_short_feet_simp
+//	==> using XML input file 
+//
+//	==> Number of joints : 12
+//
+//	==> Function : F 6 : Sensors Kinematical Informations (sens) 
+//	==> Flops complexity : 106
+//
+//	==> Generation Time :  0.000 seconds
+//	==> Post-Processing :  0.000 seconds
+//
+//-------------------------------------------------------------
+//
+ 
+#include <math.h> 
+
+#include "mbs_data.h"
+#include "mbs_project_interface.h"
+#include "mbs_sensor.h"
+ 
+void  mbs_sensor(MbsSensor *sens, 
+              MbsData *s,
+              int isens)
+{ 
+ 
+#include "mbs_sensor_coman_short_feet_simp.h" 
+#define q s->q 
+#define qd s->qd 
+#define qdd s->qdd 
+ 
+ 
+
+// === begin imp_aux === 
+
+// === end imp_aux === 
+
+// ===== BEGIN task 0 ===== 
+ 
+// Sensor Kinematics 
+
+
+
+// = = Block_0_0_0_0_0_1 = = 
+ 
+// Trigonometric Variables  
+
+  C3 = cos(q[3]);
+  S3 = sin(q[3]);
+
+// = = Block_0_0_0_0_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  C4 = cos(q[4]);
+  S4 = sin(q[4]);
+  C5 = cos(q[5]);
+  S5 = sin(q[5]);
+  C6 = cos(q[6]);
+  S6 = sin(q[6]);
+
+// = = Block_0_0_0_0_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  C7 = cos(q[7]);
+  S7 = sin(q[7]);
+  C8 = cos(q[8]);
+  S8 = sin(q[8]);
+  C9 = cos(q[9]);
+  S9 = sin(q[9]);
+
+// = = Block_0_0_0_1_0_2 = = 
+ 
+// Trigonometric Variables  
+
+  S3p4 = C3*S4+S3*C4;
+  C3p4 = C3*C4-S3*S4;
+  S3p4p5 = C3p4*S5+S3p4*C5;
+  C3p4p5 = C3p4*C5-S3p4*S5;
+  S3p4p5p6 = C3p4p5*S6+S3p4p5*C6;
+  C3p4p5p6 = C3p4p5*C6-S3p4p5*S6;
+
+// = = Block_0_0_0_2_0_3 = = 
+ 
+// Trigonometric Variables  
+
+  S3p7 = C3*S7+S3*C7;
+  C3p7 = C3*C7-S3*S7;
+  S3p7p8 = C3p7*S8+S3p7*C8;
+  C3p7p8 = C3p7*C8-S3p7*S8;
+  S3p7p8p9 = C3p7p8*S9+S3p7p8*C9;
+  C3p7p8p9 = C3p7p8*C9-S3p7p8*S9;
+
+// ====== END Task 0 ====== 
+
+// ===== BEGIN task 1 ===== 
+ 
+switch(isens)
+{
+ 
+// 
+break;
+case 1:
+ 
+
+
+// = = Block_1_0_0_1_0_2 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp0_24 = qd[3]+qd[4];
+    RLcp0_15 = s->dpt[3][4]*S3p4;
+    RLcp0_35 = s->dpt[3][4]*C3p4;
+    POcp0_25 = s->dpt[2][1]+s->dpt[2][4];
+    OMcp0_25 = OMcp0_24+qd[5];
+    RLcp0_16 = s->dpt[3][5]*S3p4p5;
+    RLcp0_36 = s->dpt[3][5]*C3p4p5;
+    OMcp0_26 = OMcp0_25+qd[6];
+    RLcp0_113 = s->dpt[3][6]*S3p4p5p6;
+    RLcp0_313 = s->dpt[3][6]*C3p4p5p6;
+    POcp0_113 = RLcp0_113+RLcp0_15+RLcp0_16+q[1];
+    POcp0_313 = RLcp0_313+RLcp0_35+RLcp0_36+q[2];
+    VIcp0_113 = qd[1]+OMcp0_24*RLcp0_35+OMcp0_25*RLcp0_36+OMcp0_26*RLcp0_313;
+    VIcp0_313 = qd[2]-OMcp0_24*RLcp0_15-OMcp0_25*RLcp0_16-OMcp0_26*RLcp0_113;
+
+// = = Block_1_0_0_1_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp0_113;
+    sens->P[2] = POcp0_25;
+    sens->P[3] = POcp0_313;
+    sens->R[1][1] = C3p4p5p6;
+    sens->R[1][3] = -S3p4p5p6;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p4p5p6;
+    sens->R[3][3] = C3p4p5p6;
+    sens->V[1] = VIcp0_113;
+    sens->V[3] = VIcp0_313;
+    sens->OM[2] = OMcp0_26;
+ 
+// 
+break;
+case 2:
+ 
+
+
+// = = Block_1_0_0_2_0_3 = = 
+ 
+// Sensor Kinematics 
+
+
+    OMcp1_27 = qd[3]+qd[7];
+    RLcp1_18 = s->dpt[3][7]*S3p7;
+    RLcp1_38 = s->dpt[3][7]*C3p7;
+    POcp1_28 = s->dpt[2][2]+s->dpt[2][7];
+    OMcp1_28 = OMcp1_27+qd[8];
+    RLcp1_19 = s->dpt[3][8]*S3p7p8;
+    RLcp1_39 = s->dpt[3][8]*C3p7p8;
+    OMcp1_29 = OMcp1_28+qd[9];
+    RLcp1_114 = s->dpt[3][9]*S3p7p8p9;
+    RLcp1_314 = s->dpt[3][9]*C3p7p8p9;
+    POcp1_114 = RLcp1_114+RLcp1_18+RLcp1_19+q[1];
+    POcp1_314 = RLcp1_314+RLcp1_38+RLcp1_39+q[2];
+    VIcp1_114 = qd[1]+OMcp1_27*RLcp1_38+OMcp1_28*RLcp1_39+OMcp1_29*RLcp1_314;
+    VIcp1_314 = qd[2]-OMcp1_27*RLcp1_18-OMcp1_28*RLcp1_19-OMcp1_29*RLcp1_114;
+
+// = = Block_1_0_0_2_1_0 = = 
+ 
+// Symbolic Outputs  
+
+    sens->P[1] = POcp1_114;
+    sens->P[2] = POcp1_28;
+    sens->P[3] = POcp1_314;
+    sens->R[1][1] = C3p7p8p9;
+    sens->R[1][3] = -S3p7p8p9;
+    sens->R[2][2] = (1.0);
+    sens->R[3][1] = S3p7p8p9;
+    sens->R[3][3] = C3p7p8p9;
+    sens->V[1] = VIcp1_114;
+    sens->V[3] = VIcp1_314;
+    sens->OM[2] = OMcp1_29;
+
+break;
+default:
+break;
+}
+
+
+// ====== END Task 1 ====== 
+
+
+}
+ 
+
diff --git a/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.h b/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.h
new file mode 100644
index 0000000..27faf6a
--- /dev/null
+++ b/symbolicR/short_feet_simp/mbs_sensor_coman_short_feet_simp.h
@@ -0,0 +1,54 @@
+double    C3;
+double    S3;
+double    C4;
+double    S4;
+double    C5;
+double    S5;
+double    C6;
+double    S6;
+double    C7;
+double    S7;
+double    C8;
+double    S8;
+double    C9;
+double    S9;
+double    S3p4;
+double    C3p4;
+double    S3p4p5;
+double    C3p4p5;
+double    S3p4p5p6;
+double    C3p4p5p6;
+double    OMcp0_24;
+double    RLcp0_15;
+double    RLcp0_35;
+double    POcp0_25;
+double    OMcp0_25;
+double    RLcp0_16;
+double    RLcp0_36;
+double    OMcp0_26;
+double    RLcp0_113;
+double    RLcp0_313;
+double    POcp0_113;
+double    POcp0_313;
+double    VIcp0_113;
+double    VIcp0_313;
+double    S3p7;
+double    C3p7;
+double    S3p7p8;
+double    C3p7p8;
+double    S3p7p8p9;
+double    C3p7p8p9;
+double    OMcp1_27;
+double    RLcp1_18;
+double    RLcp1_38;
+double    POcp1_28;
+double    OMcp1_28;
+double    RLcp1_19;
+double    RLcp1_39;
+double    OMcp1_29;
+double    RLcp1_114;
+double    RLcp1_314;
+double    POcp1_114;
+double    POcp1_314;
+double    VIcp1_114;
+double    VIcp1_314;
diff --git a/symbolicR/short_feet_simp/user_all_id.h b/symbolicR/short_feet_simp/user_all_id.h
new file mode 100644
index 0000000..d3d1c87
--- /dev/null
+++ b/symbolicR/short_feet_simp/user_all_id.h
@@ -0,0 +1,86 @@
+/* --------------------------------------------------------
+ * This code was generated automatically by MBsysC modules.
+ * MBsysC modules are distributed as part of the ROBOTRAN 
+ * software. They provides functionalities for dealing with
+ * symbolic equations generated by ROBOTRAN. 
+ *
+ * More info on www.robotran.be 
+ *
+ * Universite catholique de Louvain, Belgium 
+ *
+ * Last update : Fri Jan 27 19:12:01 2017
+ * --------------------------------------------------------
+ *
+ */
+#ifndef USER_ALL_ID_h
+#define USER_ALL_ID_h
+
+// ============================================================ //
+
+
+// joint
+
+#define FJ_T1_Coman_id 1
+#define FJ_T3_Coman_id 2
+#define FJ_R2_Coman_id 3
+#define RHipSag_id 4
+#define RKneeSag_id 5
+#define RAnkSag_id 6
+#define LHipSag_id 7
+#define LKneeSag_id 8
+#define LAnkSag_id 9
+#define WaistSag_id 10
+#define RShSag_id 11
+#define LShSag_id 12
+
+
+// body
+
+#define Waist_id 3
+#define RThigh_id 4
+#define RLowLeg_id 5
+#define RFoot_id 6
+#define LThigh_id 7
+#define LLowLeg_id 8
+#define LFoot_id 9
+#define Torso_id 10
+#define RArm_id 11
+#define LArm_id 12
+
+
+// point
+
+#define Waist_Point_0_id 1
+#define Waist_Point_1_id 2
+#define Waist_Point_2_id 3
+#define RThigh_Point_0_id 4
+#define RLowLeg_Point_0_id 5
+#define RFoot_Point_0_id 6
+#define LThigh_Point_0_id 7
+#define LLowLeg_Point_0_id 8
+#define LFoot_Point_0_id 9
+#define Torso_Point_0_id 10
+#define Torso_Point_1_id 11
+
+
+// sensor
+
+
+
+// extforce
+
+#define RFoot_Force_id 1
+#define LFoot_Force_id 2
+
+
+// links
+
+
+
+// Rods
+
+
+
+// ============================================================ //
+
+# endif
diff --git a/userFiles/ctrl/body/Hill_muscles/Muscle.cc b/userFiles/ctrl/body/Hill_muscles/Muscle.cc
index 821e004..2dbc975 100644
--- a/userFiles/ctrl/body/Hill_muscles/Muscle.cc
+++ b/userFiles/ctrl/body/Hill_muscles/Muscle.cc
@@ -99,6 +99,8 @@ Muscle::Muscle(CtrlInputs *inputs, int muscle_id, int body_part_id, double l_opt
 
 	A_filter = new LowFilter(TAU_FILTER, S, inputs->T());
 
+	A_reset = false;
+
 	last_t     = inputs->T();
 	last_t_inv = inputs->T();
 
@@ -158,8 +160,17 @@ void Muscle::Compute(StimulationCtrl *stim_ctrl)
 	S = limit_range(S, S_MIN, S_MAX);
 
 	// activation from stimulation
-	A = A_filter->UpdateGet(S, cur_t);
-
+	if (A_reset)
+	{
+		A_reset = false;
+		A = S;
+		A_filter->Reset(S, cur_t);
+	}
+	else
+	{
+		A = A_filter->UpdateGet(S, cur_t);
+	}
+	
 	// kinematics
 	RmCompute();
 	LmtuCompute();
@@ -760,3 +771,34 @@ double Muscle::InverseCompute(double Fm_des)
 
 	return limit_range(stim, S_MIN, S_MAX);
 }
+
+/*! \brief reset the controller
+ */
+void Muscle::ResetCtrl()
+{
+	A_reset = true;
+
+	n->Reset(inputs->T());
+
+	last_t     = inputs->T() - 0.001;
+	last_t_inv = inputs->T(); // not needed, just for initialization
+}
+
+/*! \brief update the muscle, based on l.ce
+ */
+void Muscle::UpdateLce()
+{
+	double cur_l_ce, cur_v_ce;
+
+	A = S_MIN;
+
+	RmCompute();
+	LmtuCompute();
+
+	cur_l_ce = l.ce;
+	cur_v_ce = v.ce;
+
+	Derivative(cur_l_ce, cur_v_ce);
+
+	v.ce = cur_v_ce;
+}
diff --git a/userFiles/ctrl/body/Hill_muscles/Muscle.hh b/userFiles/ctrl/body/Hill_muscles/Muscle.hh
index 92ec276..9cd93a5 100644
--- a/userFiles/ctrl/body/Hill_muscles/Muscle.hh
+++ b/userFiles/ctrl/body/Hill_muscles/Muscle.hh
@@ -107,6 +107,8 @@ class Muscle
 		
 		int Id() { return muscle_id; }
 
+		void ResetCtrl();
+
 		double Lce()       const { return l.ce;       }
 		double Vce()       const { return v.ce;       }
 		double Fm()        const { return F.m;        }
@@ -114,6 +116,10 @@ class Muscle
 		double Lopt()      const { return p.l_opt;    }
 		double MetEnergy() const { return met_energy; }
 
+		void SetLce(double value) { l.ce = value; }
+
+		void UpdateLce();
+
 		void Compute(StimulationCtrl *stim_ctrl);
 
 		virtual void RmCompute()   = 0;
@@ -163,6 +169,8 @@ class Muscle
 		double coef_diff_3; ///< third coefficient for the finite difference order 3
 		double coef_diff_4; ///< fourth coefficient for the finite difference order 3
 
+		bool A_reset; ///< true if activation filter must be reset, false otherwise
+
 		// function prototypes
 		void ForcesCompute(double cur_l_ce);
 		void FlCompute(double cur_l_ce);
diff --git a/userFiles/ctrl/body/WholeBody.cc b/userFiles/ctrl/body/WholeBody.cc
index 5866821..090abb8 100644
--- a/userFiles/ctrl/body/WholeBody.cc
+++ b/userFiles/ctrl/body/WholeBody.cc
@@ -257,6 +257,19 @@ void WholeBody::Compute()
 	SendReferences();
 }
 
+/*! \brief reset the control saved variables
+ */
+void WholeBody::ResetCtrl()
+{
+	stim_ctrl->ResetCtrl();
+	stim_upper->ResetCtrl();
+
+	for(int i=0; i<NB_BODY_PARTS; i++)
+	{
+		parts[i]->ResetCtrl();
+	}
+}
+
 /*! \brief send references to main controller
  */
 void WholeBody::SendReferences()
diff --git a/userFiles/ctrl/body/WholeBody.hh b/userFiles/ctrl/body/WholeBody.hh
index ae9f378..3072913 100644
--- a/userFiles/ctrl/body/WholeBody.hh
+++ b/userFiles/ctrl/body/WholeBody.hh
@@ -27,6 +27,7 @@ class WholeBody: public Computation
 		virtual ~WholeBody();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 		
 		void SendReferences();
 
@@ -46,6 +47,8 @@ class WholeBody: public Computation
 		double MetEnergyYawLegR()   const { return met_energy_yaw_legR;   }
 		double MetEnergyYawLegL()   const { return met_energy_yaw_legL;   }
 
+		BodyPart* GetPart(int i) { return parts[i]; }
+
 	private:
 		CtrlOutputs *outputs; ///< controller outputs
 
diff --git a/userFiles/ctrl/body/articulations/Articulation.hh b/userFiles/ctrl/body/articulations/Articulation.hh
index 5ce30d1..317beea 100644
--- a/userFiles/ctrl/body/articulations/Articulation.hh
+++ b/userFiles/ctrl/body/articulations/Articulation.hh
@@ -37,6 +37,8 @@ class Articulation
 
 		double ComputeQqSoft(double k_qq);
 
+		virtual void ResetCtrl() = 0;
+
 		virtual void AddQqSoftLim() = 0;
 
 		int JointId() const { return joint_id; }
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.cc
index c9ee03d..289539e 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.cc
@@ -42,3 +42,11 @@ void Pitch_Elbow_Art::AddQqSoftLim()
 	ApplyQqSoft(150.0);
 }
 
+/*! \brief reset the controller
+ */
+void Pitch_Elbow_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(150.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.hh
index 259bdfb..ae98585 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Elbow_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Elbow_Art : public Articulation
 		virtual ~Pitch_Elbow_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.cc
index d058df5..9e4190b 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.cc
@@ -42,3 +42,11 @@ void Pitch_Foot_Art::AddQqSoftLim()
 	ApplyQqSoft(135.514);
 }
 
+/*! \brief reset the controller
+ */
+void Pitch_Foot_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(135.514), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.hh
index 5e197ce..17e26ea 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Foot_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Foot_Art : public Articulation
 		virtual ~Pitch_Foot_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.cc
index 202006c..2d8032c 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.cc
@@ -41,3 +41,12 @@ void Pitch_Hip_Art::AddQqSoftLim()
 {
 	ApplyQqSoft(135.514);
 }
+
+/*! \brief reset the controller
+ */
+void Pitch_Hip_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(135.514), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.hh
index e32cbf9..1381d0c 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Hip_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Hip_Art : public Articulation
 		virtual ~Pitch_Hip_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.cc
index 2c0d28a..69478dc 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.cc
@@ -41,3 +41,12 @@ void Pitch_Knee_Art::AddQqSoftLim()
 {
 	ApplyQqSoft(135.514);
 }
+
+/*! \brief reset the controller
+ */
+void Pitch_Knee_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(135.514), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.hh
index e05a220..78d3e04 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Knee_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Knee_Art : public Articulation
 		virtual ~Pitch_Knee_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.cc
index 30183e7..f3156ff 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.cc
@@ -42,3 +42,11 @@ void Pitch_Shoulder_Art::AddQqSoftLim()
 	ApplyQqSoft(0.0);
 }
 
+/*! \brief reset the controller
+ */
+void Pitch_Shoulder_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(0.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.hh
index 07f0dc9..a935a7d 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Shoulder_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Shoulder_Art : public Articulation
 		virtual ~Pitch_Shoulder_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.cc
index 16c8216..7ec1cf8 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.cc
@@ -28,3 +28,11 @@ void Pitch_Torso_Art::AddQqSoftLim()
 	ApplyQqSoft(250.0);
 }
 
+/*! \brief reset the controller
+ */
+void Pitch_Torso_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(250.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.hh
index ce658dc..3450c9c 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Pitch_Torso_Art.hh
@@ -18,6 +18,7 @@ class Pitch_Torso_Art : public Articulation
 		virtual ~Pitch_Torso_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.cc
index dec3223..e27a656 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.cc
@@ -41,3 +41,12 @@ void Roll_Foot_Art::AddQqSoftLim()
 {
 	ApplyQqSoft(1000.0);
 }
+
+/*! \brief reset the controller
+ */
+void Roll_Foot_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(1000.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.hh
index 116cb33..885d21d 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Foot_Art.hh
@@ -18,6 +18,7 @@ class Roll_Foot_Art : public Articulation
 		virtual ~Roll_Foot_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.cc
index f716f61..4a58106 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.cc
@@ -44,3 +44,12 @@ void Roll_Hip_Art::AddQqSoftLim()
 {
 	ApplyQqSoft(1000.0);
 }
+
+/*! \brief reset the controller
+ */
+void Roll_Hip_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(1000.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.hh
index 6e62618..a7b1b22 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Hip_Art.hh
@@ -17,7 +17,8 @@ class Roll_Hip_Art : public Articulation
 		Roll_Hip_Art(CtrlInputs *inputs, MotorCtrlIndex *ctrl_index, int leg_id);
 		virtual ~Roll_Hip_Art();
 
-		virtual void AddQqSoftLim();	
+		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.cc
index b1712d7..82e583c 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.cc
@@ -43,3 +43,11 @@ void Roll_Shoulder_Art::AddQqSoftLim()
 	ApplyQqSoft(150.0);
 }
 
+/*! \brief reset the controller
+ */
+void Roll_Shoulder_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(150.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.hh
index d9eefb8..27104ba 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Shoulder_Art.hh
@@ -18,6 +18,7 @@ class Roll_Shoulder_Art : public Articulation
 		virtual ~Roll_Shoulder_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.cc
index e3d26e0..2289a0b 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.cc
@@ -28,3 +28,11 @@ void Roll_Torso_Art::AddQqSoftLim()
 	ApplyQqSoft(300.0);
 }
 
+/*! \brief reset the controller
+ */
+void Roll_Torso_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(300.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.hh
index bb809b0..3c6b49a 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Roll_Torso_Art.hh
@@ -18,6 +18,7 @@ class Roll_Torso_Art : public Articulation
 		virtual ~Roll_Torso_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.cc
index 4ef0f46..92a2125 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.cc
@@ -41,3 +41,12 @@ void Yaw_Hip_Art::AddQqSoftLim()
 {
 	ApplyQqSoft(1000.0);
 }
+
+/*! \brief reset the controller
+ */
+void Yaw_Hip_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(1000.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.hh
index f6641c3..0d645a0 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Hip_Art.hh
@@ -17,7 +17,8 @@ class Yaw_Hip_Art : public Articulation
 		Yaw_Hip_Art(CtrlInputs *inputs, MotorCtrlIndex *ctrl_index, int leg_id);
 		virtual ~Yaw_Hip_Art();
 
-		virtual void AddQqSoftLim();	
+		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.cc
index a2abc95..e780e7c 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.cc
@@ -42,3 +42,11 @@ void Yaw_Shoulder_Art::AddQqSoftLim()
 	ApplyQqSoft(250.0);
 }
 
+/*! \brief reset the controller
+ */
+void Yaw_Shoulder_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(250.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.hh
index 23a8239..5ab64df 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Shoulder_Art.hh
@@ -18,6 +18,7 @@ class Yaw_Shoulder_Art : public Articulation
 		virtual ~Yaw_Shoulder_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.cc b/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.cc
index b99a98f..6f5524d 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.cc
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.cc
@@ -28,3 +28,11 @@ void Yaw_Torso_Art::AddQqSoftLim()
 	ApplyQqSoft(150.0);
 }
 
+/*! \brief reset the controller
+ */
+void Yaw_Torso_Art::ResetCtrl()
+{
+	UpdateQQd();
+
+	filter_Qq_soft->Reset(ComputeQqSoft(150.0), inputs->T());
+}
diff --git a/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.hh b/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.hh
index e2a1ad7..dfcdd2a 100644
--- a/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.hh
+++ b/userFiles/ctrl/body/articulations/specific_art/Yaw_Torso_Art.hh
@@ -18,6 +18,7 @@ class Yaw_Torso_Art : public Articulation
 		virtual ~Yaw_Torso_Art();
 
 		virtual void AddQqSoftLim();
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/body_part/BodyPart.cc b/userFiles/ctrl/body/body_part/BodyPart.cc
index a92dd7a..2833917 100644
--- a/userFiles/ctrl/body/body_part/BodyPart.cc
+++ b/userFiles/ctrl/body/body_part/BodyPart.cc
@@ -158,3 +158,20 @@ void BodyPart::ApplyQqRefArt()
 		cur_art->SetQqRef(inputs->QqRef(cur_art->JointId()));
 	}
 }
+
+/*! \brief reset the controller
+ */
+void BodyPart::ResetCtrl()
+{
+	// reset articulations
+	for(unsigned int i=0; i<articulations.size(); i++)
+	{
+		articulations[i]->ResetCtrl();
+	}
+
+	// reset muscles
+	for(unsigned int i=0; i<muscles.size(); i++)
+	{
+		muscles[i]->ResetCtrl();
+	}
+}
diff --git a/userFiles/ctrl/body/body_part/BodyPart.hh b/userFiles/ctrl/body/body_part/BodyPart.hh
index c0f70d0..c9eccec 100644
--- a/userFiles/ctrl/body/body_part/BodyPart.hh
+++ b/userFiles/ctrl/body/body_part/BodyPart.hh
@@ -25,6 +25,8 @@ class BodyPart
 
 		virtual void UpdateSRef() = 0;
 
+		void ResetCtrl();
+
 		void UpdateInputs();
 		void ApplyQqRefArt();
 
diff --git a/userFiles/ctrl/body/delay/DelayManager.hh b/userFiles/ctrl/body/delay/DelayManager.hh
index 65529cd..1a5c083 100644
--- a/userFiles/ctrl/body/delay/DelayManager.hh
+++ b/userFiles/ctrl/body/delay/DelayManager.hh
@@ -51,6 +51,18 @@ class DelayManager
 			delay_tab[index]->NewValueTime(t, cur_val);
 		}
 
+		/*! \brief reset the delay tabular
+		 * 
+		 * \param[in] index index of the tabular
+		 * \param[in] cur_val current new value
+		 * \return current value
+		 */
+		double ResetGet(int index, double cur_val)
+		{
+			delay_tab[index]->Reset(t, cur_val);
+			return cur_val;
+		}
+
 		/*! \brief set time
 		 * 
 		 * \param[in] value new value for time [s]
diff --git a/userFiles/ctrl/body/oscillators/Oscillators.cc b/userFiles/ctrl/body/oscillators/Oscillators.cc
index 6d3041e..b6cdfca 100644
--- a/userFiles/ctrl/body/oscillators/Oscillators.cc
+++ b/userFiles/ctrl/body/oscillators/Oscillators.cc
@@ -26,6 +26,8 @@ Oscillators::Oscillators(int nb_neurons, int nb_y_out, int cur_t)
 		u.push_back(0.0);
 	}
 
+	this->nb_neurons = nb_neurons;
+
 	last_t = cur_t;
 
 	flag_init = 0;
diff --git a/userFiles/ctrl/body/oscillators/Oscillators.hh b/userFiles/ctrl/body/oscillators/Oscillators.hh
index beab04d..d8c1e6f 100644
--- a/userFiles/ctrl/body/oscillators/Oscillators.hh
+++ b/userFiles/ctrl/body/oscillators/Oscillators.hh
@@ -8,7 +8,6 @@
 #define _OSCILLATORS_HH_
 
 #include <vector>
-#include "user_realtime.h"
 
 /*! \brief Oscillators (CPG) used to provide the feedforard signals
  */
@@ -18,6 +17,8 @@ class Oscillators
 		Oscillators(int nb_neurons, int nb_y_out, int cur_t);
 		virtual ~Oscillators();
 
+		virtual void ResetCtrl() = 0;
+
 		double XPos(int index) const;
 		double YPos(int index) const;
 
@@ -46,6 +47,10 @@ class Oscillators
 			return y[index];
 		}
 
+		double NbNeurons() const { return nb_neurons; }
+
+		void SetX(int index, double value) { x[index] = value; }
+
 	protected:
 		std::vector<double> x;  ///< state of the neurons
 		std::vector<double> xd; ///< state derivative of the neurons
@@ -57,6 +62,8 @@ class Oscillators
 		int flag_init; ///< 1 if initialization done, 0 otherwise
 		int nb_y_out; ///< number of outputs
 
+		int nb_neurons; ///< number of neurons
+
 		// function prototype
 		void IntegrateNeurons(double cur_t);
 };
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.cc b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.cc
index 4567698..79c3cd9 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.cc
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.cc
@@ -35,8 +35,6 @@ Matsuoka::Matsuoka(int nb_neurons, int nb_y_out, int cur_t, double v_star, doubl
 		vd.push_back(0.0);
 	}
 
-	last_t = cur_t;
-
 	// range for speed control
 	flag_v_fwd = options->FlagVFwd();
 
@@ -73,8 +71,6 @@ void Matsuoka::IntegrateFatigue(double cur_t)
 	{
 		v[i] += vd[i] * (cur_t - last_t);
 	}
-
-	last_t = cur_t;
 }
 
 /*! \brief 'UpdateSpeed' with speed reference set to 'v_star'
@@ -162,6 +158,6 @@ void Matsuoka::UpdateSpeed(double l_req, double h_req)
 		tau_inv_v[i] = 1.0 / (gamma[i] * tau);
 	}
 
-	set_plot(tau, "tau");
-	set_output(tau, "tau");
+	//set_plot(tau, "tau");
+	//set_output(tau, "tau");
 }
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.hh b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.hh
index c592b4f..35414d4 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.hh
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka.hh
@@ -31,6 +31,10 @@ class Matsuoka: public Oscillators
 			CtrlOptions *options, StimWalkCtrl *stim_ctrl);
 		virtual ~Matsuoka();
 
+		double V(int index) const { return v[index]; }
+
+		void SetV(int index, double value) { v[index] = value; }
+
 		virtual void UpdateSpeed();
 		virtual void UpdateSpeed(double v_request);
 		virtual void UpdateSpeed(double l_req, double h_req);
@@ -137,9 +141,6 @@ class Matsuoka: public Oscillators
 		double M20_tau; ///< order 2, length
 		double M02_tau; ///< order 2, height
 
-		// integration
-		double last_t;
-
 		int flag_v_fwd; ///< flag corresponding to the forward speed tracking type
 
 		int flag_strike_err;          ///< flag to compute strike error
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.cc b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.cc
index ce4af07..5bed16b 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.cc
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.cc
@@ -108,8 +108,8 @@ void Matsuoka2D::Neurons()
 	vd[5] = tau_inv_v[0] * ( -v[5] + pos(x[5]) );
 	vd[6] = tau_inv_v[2] * ( -v[6] + pos(x[6]) );
 	vd[7] = tau_inv_v[2] * ( -v[7] + pos(x[7]) );
-	vd[8] = tau_inv_v[4] * ( -v[8] + pos(x[8]) );
-	vd[9] = tau_inv_v[4] * ( -v[9] + pos(x[9]) );
+	vd[8] = 0.0 * ( -v[8] + pos(x[8]) ); // TEMPORARY: 0.0 should be replaced by tau_inv_v[3]
+	vd[9] = 0.0 * ( -v[9] + pos(x[9]) ); // TEMPORARY: 0.0 should be replaced by tau_inv_v[3]
 
 	/*
 	set_plot(pos(x[0]), "x0");
@@ -332,8 +332,9 @@ void Matsuoka2D::Update(double cur_t)
 	
 	// Euler explicite integration
 	// IntegrateSteerNeurons(cur_t);
-	IntegrateNeurons(cur_t);
 	IntegrateFatigue(cur_t);
+	IntegrateNeurons(cur_t);
+	
 
 	// oscillators outputs
 	y[0] = pos(x[6]) - pos(x[2]); //  left  swing HFL
@@ -391,3 +392,53 @@ void Matsuoka2D::UpdateSpeed(double l_req, double h_req)
 	// mother class: tau update
 	Matsuoka::UpdateSpeed(l_req, h_req);
 }
+
+/*! \brief reset the controller
+ */
+void Matsuoka2D::ResetCtrl()
+{
+	for(unsigned int i=0; i<x.size(); i++)
+	{
+		x[i] = 0.0;
+	}
+
+	for(unsigned int i=0; i<xd.size(); i++)
+	{
+		xd[i] = 0.0;
+	}
+
+	for(unsigned int i=0; i<u.size(); i++)
+	{
+		u[i] = 0.0;
+	}
+
+	for(unsigned int i=0; i<y.size(); i++)
+	{
+		y[i] = 0.0;
+	}
+
+	for(unsigned int i=0; i<v.size(); i++)
+	{
+		v[i] = 0.0;
+	}
+
+	for(unsigned int i=0; i<vd.size(); i++)
+	{
+		vd[i] = 0.0;
+	}
+
+	last_t = inputs->T() - 0.001;
+
+	for(int i=0; i<NB_LEGS; i++)
+	{
+		flag_strike_leg[i] = 0;
+	}
+
+	flag_strike_err = 0;
+
+	flag_osc_too_fast = 0;
+	t_osc_too_fast = 0.0;
+
+	t_osc_error = 0.0;
+	t_osc_error_mean = 0.0;
+}
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.hh b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.hh
index 5bcdaf0..df9531d 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.hh
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka2D.hh
@@ -14,6 +14,8 @@ class Matsuoka2D: public Matsuoka
 		Matsuoka2D(int cur_t, double v_star, double l_star, double h_star, WalkStates *ws, CtrlInputs *inputs, CtrlOptions *options, StimWalkCtrl *stim_ctrl);
 		virtual ~Matsuoka2D();
 
+		virtual void ResetCtrl();
+
 		virtual double XSteerPos(int index) const;
 
 		virtual void UpdateSpeed();
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.cc b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.cc
index dc0dcd1..03fa69b 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.cc
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.cc
@@ -388,8 +388,9 @@ void Matsuoka3D::Update(double cur_t)
 	
 	// Euler explicite integration
 	IntegrateSteerNeurons(cur_t);
-	IntegrateNeurons(cur_t);
 	IntegrateFatigue(cur_t);
+	IntegrateNeurons(cur_t);
+	
 
 	// oscillators outputs
 	y[0] = pos(x[6])  - pos(x[2]); //  left  swing HFL
@@ -464,3 +465,10 @@ void Matsuoka3D::UpdateSpeed(double v_request)
 	// mother class: tau update
 	Matsuoka::UpdateSpeed(v_request);
 }
+
+/*! \brief reset the controller
+ */
+void Matsuoka3D::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.hh b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.hh
index c45b07c..6331503 100644
--- a/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.hh
+++ b/userFiles/ctrl/body/oscillators/specific_osc/Matsuoka3D.hh
@@ -14,6 +14,8 @@ class Matsuoka3D: public Matsuoka
 		Matsuoka3D(int cur_t, double v_star, double l_star, double h_star, WalkStates *ws, CtrlInputs *inputs, CtrlOptions *options, StimWalkCtrl *stim_ctrl);
 		virtual ~Matsuoka3D();
 
+		virtual void ResetCtrl();
+
 		virtual double XSteerPos(int index) const;
 
 		virtual void UpdateSpeed();
diff --git a/userFiles/ctrl/body/stimulations/StimulationCtrl.hh b/userFiles/ctrl/body/stimulations/StimulationCtrl.hh
index 5199f00..3b4062d 100644
--- a/userFiles/ctrl/body/stimulations/StimulationCtrl.hh
+++ b/userFiles/ctrl/body/stimulations/StimulationCtrl.hh
@@ -28,6 +28,7 @@ class StimulationCtrl
 		virtual ~StimulationCtrl();
 
 		virtual void Compute() = 0;
+		virtual void ResetCtrl() = 0;
 
 		double Stim(int leg_id, int muscle_id) { return stim[leg_id][muscle_id]; }
 
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.cc b/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.cc
index ea7c6ae..72ec661 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.cc
@@ -184,3 +184,10 @@ void StimInit::ComRef()
 {
 	x_com_ref = linear_interpol(0.01, target_x_com, t0, t1, inputs->T());
 }
+
+/*! \brief reset the controller
+ */
+void StimInit::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.hh b/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.hh
index 9824ca5..1842c28 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/init/StimInit.hh
@@ -21,6 +21,8 @@ class StimInit: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 		int IsDone() const { return flag_done; }
 
 		// opti parameters
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.cc b/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.cc
index 0572685..76f0fd5 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.cc
@@ -59,3 +59,10 @@ void StimQqRefCtrl::Compute()
 		}
 	}
 }
+
+/*! \brief reset the controller
+ */
+void StimQqRefCtrl::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.hh b/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.hh
index c5b9781..db6b5c9 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/init/StimQqRefCtrl.hh
@@ -19,6 +19,8 @@ class StimQqRefCtrl: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 	private:
 		int flag_3D; ///< 1 if 3D walking, 0 otherwise
 };
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.cc b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.cc
index ffedd2f..9a41a1f 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.cc
@@ -27,3 +27,10 @@ void StimMin::Compute()
 {
 	// nothing to do, already S_MIN
 }
+
+/*! \brief reset the controller
+ */
+void StimMin::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.hh b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.hh
index 634c204..76e0ff7 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimMin.hh
@@ -17,6 +17,8 @@ class StimMin: public StimulationCtrl
 		virtual ~StimMin();
 
 		virtual void Compute();
+
+		virtual void ResetCtrl();
 };
 
 #endif
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.cc b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.cc
index a6cf2ff..a3d3c39 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.cc
@@ -125,3 +125,10 @@ void StimTrapezoid::StimAll(double init_t, double delta_t)
 		cur_init_t += delta_t*parts[i]->NbMuscles();
 	}
 }
+
+/*! \brief reset the controller
+ */
+void StimTrapezoid::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.hh b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.hh
index b17163f..25f660c 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/tests/StimTrapezoid.hh
@@ -18,6 +18,8 @@ class StimTrapezoid: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 	private:
 		double time_A; ///< first special time [s]
 		double time_B; ///< second special time [s]
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.cc b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.cc
index 37216e2..57abdf8 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.cc
@@ -232,3 +232,10 @@ void StimUpperPos::PCtrl(double q_ref, double q_ref_d, int body_part_id, int art
 			exit(EXIT_FAILURE);
 	}
 }
+
+/*! \brief reset the controller
+ */
+void StimUpperPos::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.hh b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.hh
index 1ebb60a..6449d4b 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperPos.hh
@@ -18,6 +18,8 @@ class StimUpperPos: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 	private:
 		MotorCtrlIndex *ctrl_index; ///< controller indexes
 
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.cc b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.cc
index 11fe2e2..b71a313 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.cc
@@ -292,3 +292,10 @@ void StimUpperWalk::PCtrl(double q_ref, int body_part_id, int art_id)
 			exit(EXIT_FAILURE);
 	}
 }
+
+/*! \brief reset the controller
+ */
+void StimUpperWalk::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.hh b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.hh
index efd5f14..652fd4b 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/upper/StimUpperWalk.hh
@@ -24,6 +24,8 @@ class StimUpperWalk: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 		void SetKTorso(double value) { k_torso = value; }
 
 	private:
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.cc b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.cc
index 06d45c9..4c3d5e1 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.cc
@@ -133,6 +133,13 @@ StimReflexCtrl::~StimReflexCtrl()
 	delete delay_manager;
 }
 
+/*! \brief reset the controller
+ */
+void StimReflexCtrl::ResetCtrl()
+{
+
+}
+
 /*! \brief main computation
  */
 void StimReflexCtrl::Compute()
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.hh b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.hh
index 6a34cbf..e046fb5 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimReflexCtrl.hh
@@ -22,6 +22,8 @@ class StimReflexCtrl: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 		void ComputeDelay();
 		void ComputeStim();
 
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.cc b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.cc
index 4ccc352..716877e 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.cc
+++ b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.cc
@@ -51,6 +51,8 @@ StimWalkCtrl::StimWalkCtrl(CtrlInputs *inputs, WalkStates *ws, ForwardKinematics
 
 	delay_manager = new DelayWalk();
 
+	delay_reset = 0;
+
 	// speed tracking
 	this->v_star = v_star;
 	v_request = v_star;
@@ -260,13 +262,13 @@ StimWalkCtrl::StimWalkCtrl(CtrlInputs *inputs, WalkStates *ws, ForwardKinematics
 	// speed tracking init
 	UpdateSpeed();
 
-	set_plot(user_ctr->S_length(), "length ref");
-	set_plot(user_ctr->S_height(), "height ref");
-	// set_plot(k_HAM1, "k_HAM1");
-	// set_plot(k_HAM2, "k_HAM2");
-	// set_plot(k_GLU1, "k_GLU1");
-	// set_plot(k_HFL,  "k_HFL");
-	set_plot(theta_ref,"k_theta");
+	//set_plot(user_ctr->S_length(), "length ref");
+	//set_plot(user_ctr->S_height(), "height ref");
+	//set_plot(k_HAM1, "k_HAM1");
+	//set_plot(k_HAM2, "k_HAM2");
+	//set_plot(k_GLU1, "k_GLU1");
+	//set_plot(k_HFL,  "k_HFL");
+	//set_plot(theta_ref,"k_theta");
 
 	// for optimization
 	inputs->OptiIn()->SetStimLower(this);
@@ -281,17 +283,26 @@ StimWalkCtrl::~StimWalkCtrl()
 	delete delay_manager;
 }
 
+/*! \brief reset the controller
+ */
+void StimWalkCtrl::ResetCtrl()
+{
+	delay_reset = 1;
+
+	osc->ResetCtrl();
+}
+
 /*! \brief main computation
  */
 void StimWalkCtrl::Compute()
 {
-	set_plot(user_ctr->S_length(), "length ref");
-	set_plot(user_ctr->S_height(), "height ref");
-	// set_plot(k_HAM1, "k_HAM1");
-	// set_plot(k_HAM2, "k_HAM2");
-	// set_plot(k_GLU1, "k_GLU1");
-	// set_plot(k_HFL,  "k_HFL");
-	set_plot(theta_ref,"k_theta");
+	//set_plot(user_ctr->S_length(), "length ref");
+	//set_plot(user_ctr->S_height(), "height ref");
+	//set_plot(k_HAM1, "k_HAM1");
+	//set_plot(k_HAM2, "k_HAM2");
+	//set_plot(k_GLU1, "k_GLU1");
+	//set_plot(k_HFL,  "k_HFL");
+	//set_plot(theta_ref,"k_theta");
 
 	ComputeDelay();
 
@@ -326,14 +337,14 @@ void StimWalkCtrl::Compute()
 		}
 	}
 
-	set_output(user_ctr->S_length(), "sl_ref");
-	set_output(user_ctr->S_height(), "sh_ref");
+	//set_output(user_ctr->S_length(), "sl_ref");
+	//set_output(user_ctr->S_height(), "sh_ref");
 
-	set_output(k_HFL, "k_HFL");
-	set_output(k_GLU1, "k_GLU1");
-	set_output(k_HAM1, "k_HAM1");
-	set_output(k_HAM2, "k_HAM2");
-	set_output(theta_ref, "theta_ref");
+	//set_output(k_HFL, "k_HFL");
+	//set_output(k_GLU1, "k_GLU1");
+	//set_output(k_HAM1, "k_HAM1");
+	//set_output(k_HAM2, "k_HAM2");
+	//set_output(theta_ref, "theta_ref");
 	
 	ComputeStim();
 }
@@ -345,71 +356,143 @@ void StimWalkCtrl::ComputeDelay()
 	// Update time
 	delay_manager->SetT(inputs->T());
 
-	// -- Pitch -- //
+	if (delay_reset > 0)
+	{
+		delay_reset = (delay_reset > 1) ? 0 : 2;
+
+		// -- Pitch -- //
+
+		// feet forces
+		F_foot[R_ID] = delay_manager->ResetGet(F_FOOT_R, inputs->MeanFzFeet(R_ID));
+		F_foot[L_ID] = delay_manager->ResetGet(F_FOOT_L, inputs->MeanFzFeet(L_ID));
+
+		// angles
+		phi_kp[R_ID] = delay_manager->ResetGet(PHI_K_R, pitch_knee[R_ID]->Q());
+		phi_kp[L_ID] = delay_manager->ResetGet(PHI_K_L, pitch_knee[L_ID]->Q());
+		phi_dot_kp[R_ID] = delay_manager->ResetGet(PHI_DOT_K_R, pitch_knee[R_ID]->Qd());
+		phi_dot_kp[L_ID] = delay_manager->ResetGet(PHI_DOT_K_L, pitch_knee[L_ID]->Qd());
+
+		// l.ce
+		lce_ta[R_ID]  = delay_manager->ResetGet(LCE_TA_R, ta[R_ID]->Lce());
+		lce_ta[L_ID]  = delay_manager->ResetGet(LCE_TA_L, ta[L_ID]->Lce());
+		lce_vas[R_ID] = delay_manager->ResetGet(LCE_VAS_R, vas[R_ID]->Lce());
+		lce_vas[L_ID] = delay_manager->ResetGet(LCE_VAS_L, vas[L_ID]->Lce());
+
+		// Fm
+		F_sol[R_ID] = delay_manager->ResetGet(F_SOL_R, sol[R_ID]->Fm());
+		F_sol[L_ID] = delay_manager->ResetGet(F_SOL_L, sol[L_ID]->Fm());
+		F_gas[R_ID] = delay_manager->ResetGet(F_GAS_R, gas[R_ID]->Fm());
+		F_gas[L_ID] = delay_manager->ResetGet(F_GAS_L, gas[L_ID]->Fm());
+		F_vas[R_ID] = delay_manager->ResetGet(F_VAS_R, vas[R_ID]->Fm());
+		F_vas[L_ID] = delay_manager->ResetGet(F_VAS_L, vas[L_ID]->Fm());
+
+		// torso
+		theta_torso = delay_manager->ResetGet(THETA_TORSO, inputs->ThetaTorso(1));
+		theta_dot_torso = delay_manager->ResetGet(THETA_DOT_TORSO, inputs->OmegaTorso(1));
+
+		// -- Roll -- //
 
-	// feet forces
-	F_foot[R_ID] = delay_manager->UpdateGet(F_FOOT_R, inputs->MeanFzFeet(R_ID));
-	F_foot[L_ID] = delay_manager->UpdateGet(F_FOOT_L, inputs->MeanFzFeet(L_ID));
+		// angles
+		phi_hr[R_ID] = delay_manager->ResetGet(PHI_ROLL_HIP_R, roll_hip[R_ID]->Q());
+		phi_hr[L_ID] = delay_manager->ResetGet(PHI_ROLL_HIP_L, roll_hip[L_ID]->Q());
 
-	// angles
-	phi_kp[R_ID] = delay_manager->UpdateGet(PHI_K_R, pitch_knee[R_ID]->Q());
-	phi_kp[L_ID] = delay_manager->UpdateGet(PHI_K_L, pitch_knee[L_ID]->Q());
-	phi_dot_kp[R_ID] = delay_manager->UpdateGet(PHI_DOT_K_R, pitch_knee[R_ID]->Qd());
-	phi_dot_kp[L_ID] = delay_manager->UpdateGet(PHI_DOT_K_L, pitch_knee[L_ID]->Qd());
+		phi_dot_hr[R_ID] = delay_manager->ResetGet(PHI_DOT_ROLL_HIP_R, roll_hip[R_ID]->Qd());
+		phi_dot_hr[L_ID] = delay_manager->ResetGet(PHI_DOT_ROLL_HIP_L, roll_hip[L_ID]->Qd());
 
-	// l.ce
-	lce_ta[R_ID]  = delay_manager->UpdateGet(LCE_TA_R, ta[R_ID]->Lce());
-	lce_ta[L_ID]  = delay_manager->UpdateGet(LCE_TA_L, ta[L_ID]->Lce());
-	lce_vas[R_ID] = delay_manager->UpdateGet(LCE_VAS_R, vas[R_ID]->Lce());
-	lce_vas[L_ID] = delay_manager->UpdateGet(LCE_VAS_L, vas[L_ID]->Lce());
+		// com
+		y_com_Rfoot = delay_manager->ResetGet(Y_COM_R, fwd_kin->RComRFootYaw(1));
+		y_com_Lfoot = delay_manager->ResetGet(Y_COM_L, fwd_kin->RComLFootYaw(1));
 
-	// Fm
-	F_sol[R_ID] = delay_manager->UpdateGet(F_SOL_R, sol[R_ID]->Fm());
-	F_sol[L_ID] = delay_manager->UpdateGet(F_SOL_L, sol[L_ID]->Fm());
-	F_gas[R_ID] = delay_manager->UpdateGet(F_GAS_R, gas[R_ID]->Fm());
-	F_gas[L_ID] = delay_manager->UpdateGet(F_GAS_L, gas[L_ID]->Fm());
-	F_vas[R_ID] = delay_manager->UpdateGet(F_VAS_R, vas[R_ID]->Fm());
-	F_vas[L_ID] = delay_manager->UpdateGet(F_VAS_L, vas[L_ID]->Fm());
+		yp_com_Rfoot = delay_manager->ResetGet(Y_DOT_COM_R, fwd_kin->RpComRFootYaw(1));
+		yp_com_Lfoot = delay_manager->ResetGet(Y_DOT_COM_L, fwd_kin->RpComLFootYaw(1));
 
-	// torso
-	theta_torso = delay_manager->UpdateGet(THETA_TORSO, inputs->ThetaTorso(1));
-	theta_dot_torso = delay_manager->UpdateGet(THETA_DOT_TORSO, inputs->OmegaTorso(1));
+		// roll absolute angles
+		psi_torso = delay_manager->ResetGet(PSI_TORSO, inputs->ThetaTorso(0));
+		psi_Rfoot = delay_manager->ResetGet(PSI_R_FOOT, inputs->ThetaRFoot(0));
+		psi_Lfoot = delay_manager->ResetGet(PSI_L_FOOT, inputs->ThetaLFoot(0));
 
+		// roll absolute angles derivative
+		psi_dot_torso = delay_manager->ResetGet(PSI_DOT_TORSO, inputs->OmegaTorso(0));
+		psi_dot_Rfoot = delay_manager->ResetGet(PSI_DOT_R_FOOT, inputs->OmegaRFoot(0));
+		psi_dot_Lfoot = delay_manager->ResetGet(PSI_DOT_L_FOOT, inputs->OmegaLFoot(0));
 
-	// -- Roll -- //
 
-	// angles
-	phi_hr[R_ID] = delay_manager->UpdateGet(PHI_ROLL_HIP_R, roll_hip[R_ID]->Q());
-	phi_hr[L_ID] = delay_manager->UpdateGet(PHI_ROLL_HIP_L, roll_hip[L_ID]->Q());
+		// -- Yaw -- //
 
-	phi_dot_hr[R_ID] = delay_manager->UpdateGet(PHI_DOT_ROLL_HIP_R, roll_hip[R_ID]->Qd());
-	phi_dot_hr[L_ID] = delay_manager->UpdateGet(PHI_DOT_ROLL_HIP_L, roll_hip[L_ID]->Qd());
+		phi_hy[R_ID] = delay_manager->ResetGet(PHI_YAW_HIP_R, yaw_hip[R_ID]->Q());
+		phi_hy[L_ID] = delay_manager->ResetGet(PHI_YAW_HIP_L, yaw_hip[L_ID]->Q());
 
-	// com
-	y_com_Rfoot = delay_manager->UpdateGet(Y_COM_R, fwd_kin->RComRFootYaw(1));
-	y_com_Lfoot = delay_manager->UpdateGet(Y_COM_L, fwd_kin->RComLFootYaw(1));
+		phi_dot_hy[R_ID] = delay_manager->ResetGet(PHI_DOT_YAW_HIP_R, yaw_hip[R_ID]->Qd());
+		phi_dot_hy[L_ID] = delay_manager->ResetGet(PHI_DOT_YAW_HIP_L, yaw_hip[L_ID]->Qd());
+	}
+	else
+	{
+		// -- Pitch -- //
+
+		// feet forces
+		F_foot[R_ID] = delay_manager->UpdateGet(F_FOOT_R, inputs->MeanFzFeet(R_ID));
+		F_foot[L_ID] = delay_manager->UpdateGet(F_FOOT_L, inputs->MeanFzFeet(L_ID));
+
+		// angles
+		phi_kp[R_ID] = delay_manager->UpdateGet(PHI_K_R, pitch_knee[R_ID]->Q());
+		phi_kp[L_ID] = delay_manager->UpdateGet(PHI_K_L, pitch_knee[L_ID]->Q());
+		phi_dot_kp[R_ID] = delay_manager->UpdateGet(PHI_DOT_K_R, pitch_knee[R_ID]->Qd());
+		phi_dot_kp[L_ID] = delay_manager->UpdateGet(PHI_DOT_K_L, pitch_knee[L_ID]->Qd());
+
+		// l.ce
+		lce_ta[R_ID]  = delay_manager->UpdateGet(LCE_TA_R, ta[R_ID]->Lce());
+		lce_ta[L_ID]  = delay_manager->UpdateGet(LCE_TA_L, ta[L_ID]->Lce());
+		lce_vas[R_ID] = delay_manager->UpdateGet(LCE_VAS_R, vas[R_ID]->Lce());
+		lce_vas[L_ID] = delay_manager->UpdateGet(LCE_VAS_L, vas[L_ID]->Lce());
+
+		// Fm
+		F_sol[R_ID] = delay_manager->UpdateGet(F_SOL_R, sol[R_ID]->Fm());
+		F_sol[L_ID] = delay_manager->UpdateGet(F_SOL_L, sol[L_ID]->Fm());
+		F_gas[R_ID] = delay_manager->UpdateGet(F_GAS_R, gas[R_ID]->Fm());
+		F_gas[L_ID] = delay_manager->UpdateGet(F_GAS_L, gas[L_ID]->Fm());
+		F_vas[R_ID] = delay_manager->UpdateGet(F_VAS_R, vas[R_ID]->Fm());
+		F_vas[L_ID] = delay_manager->UpdateGet(F_VAS_L, vas[L_ID]->Fm());
 
-	yp_com_Rfoot = delay_manager->UpdateGet(Y_DOT_COM_R, fwd_kin->RpComRFootYaw(1));
-	yp_com_Lfoot = delay_manager->UpdateGet(Y_DOT_COM_L, fwd_kin->RpComLFootYaw(1));
+		// torso
+		theta_torso = delay_manager->UpdateGet(THETA_TORSO, inputs->ThetaTorso(1));
+		theta_dot_torso = delay_manager->UpdateGet(THETA_DOT_TORSO, inputs->OmegaTorso(1));
 
-	// roll absolute angles
-	psi_torso = delay_manager->UpdateGet(PSI_TORSO, inputs->ThetaTorso(0));
-	psi_Rfoot = delay_manager->UpdateGet(PSI_R_FOOT, inputs->ThetaRFoot(0));
-	psi_Lfoot = delay_manager->UpdateGet(PSI_L_FOOT, inputs->ThetaLFoot(0));
 
-	// roll absolute angles derivative
-	psi_dot_torso = delay_manager->UpdateGet(PSI_DOT_TORSO, inputs->OmegaTorso(0));
-	psi_dot_Rfoot = delay_manager->UpdateGet(PSI_DOT_R_FOOT, inputs->OmegaRFoot(0));
-	psi_dot_Lfoot = delay_manager->UpdateGet(PSI_DOT_L_FOOT, inputs->OmegaLFoot(0));
+		// -- Roll -- //
 
+		// angles
+		phi_hr[R_ID] = delay_manager->UpdateGet(PHI_ROLL_HIP_R, roll_hip[R_ID]->Q());
+		phi_hr[L_ID] = delay_manager->UpdateGet(PHI_ROLL_HIP_L, roll_hip[L_ID]->Q());
 
-	// -- Yaw -- //
+		phi_dot_hr[R_ID] = delay_manager->UpdateGet(PHI_DOT_ROLL_HIP_R, roll_hip[R_ID]->Qd());
+		phi_dot_hr[L_ID] = delay_manager->UpdateGet(PHI_DOT_ROLL_HIP_L, roll_hip[L_ID]->Qd());
 
-	phi_hy[R_ID] = delay_manager->UpdateGet(PHI_YAW_HIP_R, yaw_hip[R_ID]->Q());
-	phi_hy[L_ID] = delay_manager->UpdateGet(PHI_YAW_HIP_L, yaw_hip[L_ID]->Q());
+		// com
+		y_com_Rfoot = delay_manager->UpdateGet(Y_COM_R, fwd_kin->RComRFootYaw(1));
+		y_com_Lfoot = delay_manager->UpdateGet(Y_COM_L, fwd_kin->RComLFootYaw(1));
 
-	phi_dot_hy[R_ID] = delay_manager->UpdateGet(PHI_DOT_YAW_HIP_R, yaw_hip[R_ID]->Qd());
-	phi_dot_hy[L_ID] = delay_manager->UpdateGet(PHI_DOT_YAW_HIP_L, yaw_hip[L_ID]->Qd());
+		yp_com_Rfoot = delay_manager->UpdateGet(Y_DOT_COM_R, fwd_kin->RpComRFootYaw(1));
+		yp_com_Lfoot = delay_manager->UpdateGet(Y_DOT_COM_L, fwd_kin->RpComLFootYaw(1));
+
+		// roll absolute angles
+		psi_torso = delay_manager->UpdateGet(PSI_TORSO, inputs->ThetaTorso(0));
+		psi_Rfoot = delay_manager->UpdateGet(PSI_R_FOOT, inputs->ThetaRFoot(0));
+		psi_Lfoot = delay_manager->UpdateGet(PSI_L_FOOT, inputs->ThetaLFoot(0));
+
+		// roll absolute angles derivative
+		psi_dot_torso = delay_manager->UpdateGet(PSI_DOT_TORSO, inputs->OmegaTorso(0));
+		psi_dot_Rfoot = delay_manager->UpdateGet(PSI_DOT_R_FOOT, inputs->OmegaRFoot(0));
+		psi_dot_Lfoot = delay_manager->UpdateGet(PSI_DOT_L_FOOT, inputs->OmegaLFoot(0));
+
+
+		// -- Yaw -- //
+
+		phi_hy[R_ID] = delay_manager->UpdateGet(PHI_YAW_HIP_R, yaw_hip[R_ID]->Q());
+		phi_hy[L_ID] = delay_manager->UpdateGet(PHI_YAW_HIP_L, yaw_hip[L_ID]->Q());
+
+		phi_dot_hy[R_ID] = delay_manager->UpdateGet(PHI_DOT_YAW_HIP_R, yaw_hip[R_ID]->Qd());
+		phi_dot_hy[L_ID] = delay_manager->UpdateGet(PHI_DOT_YAW_HIP_L, yaw_hip[L_ID]->Qd());
+	}
 }
 
 /*! \brief Update the oscillators
diff --git a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.hh b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.hh
index 0011eef..79e74bc 100644
--- a/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.hh
+++ b/userFiles/ctrl/body/stimulations/specific_stim/walk/StimWalkCtrl.hh
@@ -29,6 +29,8 @@ class StimWalkCtrl: public StimulationCtrl
 
 		virtual void Compute();
 
+		virtual void ResetCtrl();
+
 		Matsuoka* Osc() { return osc; }
 
 		// pitch opti
@@ -389,6 +391,9 @@ class StimWalkCtrl: public StimulationCtrl
 		double period_ref;
 		double adapt_params;
 
+		// reset the controller
+		int delay_reset; ///< positive if delay must be reset
+
 		// functions prototypes
 		void ComputeDelay();
 		void UpdateOsc();
diff --git a/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.cc b/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.cc
index 527dfaa..32f6f80 100644
--- a/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.cc
+++ b/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.cc
@@ -137,3 +137,10 @@ void ImpedanceCtrlInit::Compute()
 	// main computation
 	ImpedanceCtrl::Compute();
 }
+
+/*! \brief reset the control saved variables
+ */
+void ImpedanceCtrlInit::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.hh b/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.hh
index 3e99ca0..55a1b24 100644
--- a/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.hh
+++ b/userFiles/ctrl/impedance/specific_imp/ImpedanceCtrlInit.hh
@@ -18,6 +18,7 @@ class ImpedanceCtrlInit: public ImpedanceCtrlStanding
 		virtual ~ImpedanceCtrlInit();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 		void SetXRefEnd(double value) { x_ref_end = value; }
 		void SetYRefEnd(double value) { y_ref_end = value; }
diff --git a/userFiles/ctrl/io/ctrl_options/CtrlOptions.hh b/userFiles/ctrl/io/ctrl_options/CtrlOptions.hh
index a81afd4..aae5538 100644
--- a/userFiles/ctrl/io/ctrl_options/CtrlOptions.hh
+++ b/userFiles/ctrl/io/ctrl_options/CtrlOptions.hh
@@ -7,10 +7,6 @@
 #ifndef _CTRL_OPTIONS_HH_
 #define _CTRL_OPTIONS_HH_
 
-#ifdef ROBOTRAN_SIMU
-	#include "user_realtime.h"
-#endif
-
 /// controller choice
 enum{CTRL_DEFAULT, CTRL_REFLEX, CTRL_CPG, CTRL_STIM_TEST};
 
diff --git a/userFiles/ctrl/io/handle/HandleInputs.cc b/userFiles/ctrl/io/handle/HandleInputs.cc
index a947699..8b179e5 100644
--- a/userFiles/ctrl/io/handle/HandleInputs.cc
+++ b/userFiles/ctrl/io/handle/HandleInputs.cc
@@ -45,6 +45,23 @@ void HandleInputs::Compute()
 	FeetForces();
 }
 
+/*! \brief reset the control saved variables
+ */
+void HandleInputs::ResetCtrl()
+{
+	// Vertical forces [N]
+	inputs->SetFzFeet(R_ID, limit_range( fabs(inputs->FRFootIF(2)), -MAX_EXT_FORCE_Z_CONTROLLER, MAX_EXT_FORCE_Z_CONTROLLER) );
+	inputs->SetFzFeet(L_ID, limit_range( fabs(inputs->FLFootIF(2)), -MAX_EXT_FORCE_Z_CONTROLLER, MAX_EXT_FORCE_Z_CONTROLLER) );
+
+	// mean forces (reset running average)
+	for(int i=0; i<NB_LEGS; i++)
+	{
+		inputs->SetMeanFzFeet(i, inputs->FzFeet(i));
+
+		feet_av_compute[i]->Reset(inputs->FzFeet(i));
+	}
+}
+
 /*! \brief feet forces limited and with running average low-pass filter
  */
 void HandleInputs::FeetForces()
diff --git a/userFiles/ctrl/io/handle/HandleInputs.hh b/userFiles/ctrl/io/handle/HandleInputs.hh
index 00f3f50..d827668 100644
--- a/userFiles/ctrl/io/handle/HandleInputs.hh
+++ b/userFiles/ctrl/io/handle/HandleInputs.hh
@@ -47,6 +47,7 @@ class HandleInputs: public Computation
 		virtual ~HandleInputs();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 	private:
 		ForwardKinematics *fwd_kin; ///< foward kinematics
diff --git a/userFiles/ctrl/io/handle/HandleOutputs.cc b/userFiles/ctrl/io/handle/HandleOutputs.cc
index a3739fb..1f1063e 100644
--- a/userFiles/ctrl/io/handle/HandleOutputs.cc
+++ b/userFiles/ctrl/io/handle/HandleOutputs.cc
@@ -47,3 +47,10 @@ void HandleOutputs::Compute()
 		}
 	}
 }
+
+/*! \brief reset the control saved variables
+ */
+void HandleOutputs::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/io/handle/HandleOutputs.hh b/userFiles/ctrl/io/handle/HandleOutputs.hh
index a37870d..b914148 100644
--- a/userFiles/ctrl/io/handle/HandleOutputs.hh
+++ b/userFiles/ctrl/io/handle/HandleOutputs.hh
@@ -20,6 +20,7 @@ class HandleOutputs: public Computation
 		virtual ~HandleOutputs();
 
 		void Compute();
+		virtual void ResetCtrl();
 
 	private:
 		CtrlOutputs *outputs; ///< outputs of the controller
diff --git a/userFiles/ctrl/kinematic/ForwardKinematics.cc b/userFiles/ctrl/kinematic/ForwardKinematics.cc
index 749966f..dba6be8 100644
--- a/userFiles/ctrl/kinematic/ForwardKinematics.cc
+++ b/userFiles/ctrl/kinematic/ForwardKinematics.cc
@@ -247,6 +247,13 @@ void ForwardKinematics::Compute()
 	ComFeet();
 }
 
+/*! \brief reset the control saved variables
+ */
+void ForwardKinematics::ResetCtrl()
+{
+
+}
+
 /*! \brief checks related to the robot state<s
  */
 void ForwardKinematics::StateCheck()
diff --git a/userFiles/ctrl/kinematic/ForwardKinematics.hh b/userFiles/ctrl/kinematic/ForwardKinematics.hh
index 62259b8..4a73676 100644
--- a/userFiles/ctrl/kinematic/ForwardKinematics.hh
+++ b/userFiles/ctrl/kinematic/ForwardKinematics.hh
@@ -116,6 +116,7 @@ class ForwardKinematics: public Computation
 		virtual ~ForwardKinematics();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 		// robot forward frame
 		double RComRFootYaw(int i)  const { return r_COM_Rfoot_yaw[i]; }
diff --git a/userFiles/ctrl/kinematic/OdometryKinematics.cc b/userFiles/ctrl/kinematic/OdometryKinematics.cc
index e8323d6..cba7eb3 100644
--- a/userFiles/ctrl/kinematic/OdometryKinematics.cc
+++ b/userFiles/ctrl/kinematic/OdometryKinematics.cc
@@ -137,6 +137,13 @@ void OdometryKinematics::Compute()
 	MainKin(gen_in_out);
 }
 
+/*! \brief reset the control saved variables
+ */
+void OdometryKinematics::ResetCtrl()
+{
+
+}
+
 /*! \brief fill the generic inputs
  *
  * \param[in,out] in_out inputs and outputs class
diff --git a/userFiles/ctrl/kinematic/OdometryKinematics.hh b/userFiles/ctrl/kinematic/OdometryKinematics.hh
index c72f5f0..8f0fcf1 100644
--- a/userFiles/ctrl/kinematic/OdometryKinematics.hh
+++ b/userFiles/ctrl/kinematic/OdometryKinematics.hh
@@ -73,6 +73,7 @@ class OdometryKinematics: public Computation
 		virtual ~OdometryKinematics();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 		void FillGenInputs(OdometryInOut &in_out);
 		void MainKin(OdometryInOut &in_out);
diff --git a/userFiles/ctrl/main/coman_properties.hh b/userFiles/ctrl/main/coman_properties.hh
index eae8230..c23a255 100644
--- a/userFiles/ctrl/main/coman_properties.hh
+++ b/userFiles/ctrl/main/coman_properties.hh
@@ -13,6 +13,8 @@
 
 #define NB_MOTORS 23 ///< number of motors
 
+#define TIME_SWITCH_TAKE_OFF 0.05 ///< time to apply a switch of the parallel view, after a take-off [s]
+
 enum{R_ID, L_ID};
 
 #define	NB_LEGS 2
diff --git a/userFiles/ctrl/main/computation/Computation.hh b/userFiles/ctrl/main/computation/Computation.hh
index f142e4a..f0be22c 100644
--- a/userFiles/ctrl/main/computation/Computation.hh
+++ b/userFiles/ctrl/main/computation/Computation.hh
@@ -21,6 +21,8 @@ class Computation
 
 		virtual void Compute() = 0;
 
+		virtual void ResetCtrl() = 0;
+
 	protected:
 		CtrlInputs *inputs;      ///< controller inputs
 		CtrlOptions *options;    ///< controller options
diff --git a/userFiles/ctrl/main/computation/ComputeManager.cc b/userFiles/ctrl/main/computation/ComputeManager.cc
index 6619a12..37a96ba 100644
--- a/userFiles/ctrl/main/computation/ComputeManager.cc
+++ b/userFiles/ctrl/main/computation/ComputeManager.cc
@@ -2,6 +2,7 @@
 #include "ComputeManager.hh"
 #include "HandleInputs.hh"
 #include "HandleOutputs.hh"
+#include "SwitchData.hh"
 #include "WalkStates.hh"
 #include "ForwardKinematics.hh"
 #include "OdometryKinematics.hh"
@@ -19,9 +20,10 @@
  */
 ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOptions *options, MotorCtrlIndex *ctrl_index)
 {
-	int flag_fwd_kin, flag_odo_kin, flag_ws;
+	int flag_fwd_kin, flag_odo_kin, flag_ws, flag_wb;
 
 	WalkStates *ws;
+	WholeBody *wb;
 	ForwardKinematics *fwd_kin;
 	OdometryKinematics *odo_kin;
 
@@ -33,9 +35,11 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 	flag_fwd_kin = 0;
 	flag_odo_kin = 0;
 	flag_ws = 0;
+	flag_wb = 0;
 
 	fwd_kin = NULL;
 	ws = NULL;
+	wb = NULL;
 
 	// computation modules to activate
 	active_mod.push_back(WALK_STATES);
@@ -46,6 +50,7 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 	//active_mod.push_back(UPPER_BODY);
 	active_mod.push_back(IMPEDANCE_CTRL);
 	active_mod.push_back(HANDLE_OUTPUTS);
+	active_mod.push_back(SWITCH_DATA);
 
 	for(unsigned int i=0; i<active_mod.size(); i++)
 	{
@@ -62,7 +67,7 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 			case KINEMATICS_COMPUTE:
 				if (!flag_ws)
 				{
-					std::cout << "Error: WalkStates not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: WalkStates not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				fwd_kin = new ForwardKinematics(inputs, options, ctrl_index, ws);
@@ -74,7 +79,7 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 			case ODOMETRY_COMPUTE:
 				if (!flag_ws)
 				{
-					std::cout << "Error: WalkStates not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: WalkStates not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				odo_kin = new OdometryKinematics(inputs, options, ctrl_index, ws);
@@ -86,7 +91,7 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 			case HANDLE_INPUTS:
 				if (!flag_fwd_kin)
 				{
-					std::cout << "Error: ForwardKinematics not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: ForwardKinematics not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				compute_tab.push_back(new HandleInputs(inputs, options, ctrl_index, fwd_kin));
@@ -96,22 +101,24 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 			case WHOLE_BODY:
 				if (!flag_ws)
 				{
-					std::cout << "Error: WalkStates not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: WalkStates not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				if (!flag_fwd_kin)
 				{
-					std::cout << "Error: ForwardKinematics not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: ForwardKinematics not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
-				compute_tab.push_back(new WholeBody(inputs, options, ctrl_index, outputs, ws, fwd_kin));
+				wb = new WholeBody(inputs, options, ctrl_index, outputs, ws, fwd_kin);
+				compute_tab.push_back(wb);
+				flag_wb = 1;
 				break;
 
 			// upper body
 			case UPPER_BODY:
 				if (!flag_ws)
 				{
-					std::cout << "Error: WalkStates not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: WalkStates not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				compute_tab.push_back(new UpperBody(inputs, options, ctrl_index, outputs, ws));
@@ -121,7 +128,7 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 			case IMPEDANCE_CTRL:
 				if (!flag_fwd_kin)
 				{
-					std::cout << "Error: ForwardKinematics not initialized !" << std::endl;
+					std::cout << "ComputeManager Error: ForwardKinematics not initialized !" << std::endl;
 					exit(EXIT_FAILURE);
 				}
 				compute_tab.push_back(new ImpedanceCtrlInit(inputs, options, ctrl_index, outputs, fwd_kin));
@@ -132,8 +139,25 @@ ComputeManager::ComputeManager(CtrlInputs *inputs, CtrlOutputs *outputs, CtrlOpt
 				compute_tab.push_back(new HandleOutputs(inputs, options, ctrl_index, outputs));
 				break;
 
+			// switch data (for parallel simu)
+			case SWITCH_DATA:
+				#ifndef PARALLEL_SIMU
+				if (!flag_ws)
+				{
+					std::cout << "ComputeManager Error: WalkStates not initialized !" << std::endl;
+					exit(EXIT_FAILURE);
+				}
+				if (!flag_wb)
+				{
+					std::cout << "ComputeManager Error: WholeBody not initialized !" << std::endl;
+					exit(EXIT_FAILURE);
+				}
+				compute_tab.push_back(new SwitchData(inputs, options, ctrl_index, ws, wb));
+				#endif
+				break;
+
 			default:
-				std::cout << "Error: unknown computation model !" << std::endl;
+				std::cout << "ComputeManager Error: unknown computation model !" << std::endl;
 				exit(EXIT_FAILURE);
 				break;
 		}
@@ -180,3 +204,13 @@ void ComputeManager::RunComputations()
 		compute_tab[i]->Compute();
 	}
 }
+
+/*! \brief reset the control saved variables
+ */
+void ComputeManager::ResetCtrl()
+{
+	for(unsigned int i=0; i<compute_tab.size(); i++)
+	{
+		compute_tab[i]->ResetCtrl();
+	}
+}
diff --git a/userFiles/ctrl/main/computation/ComputeManager.hh b/userFiles/ctrl/main/computation/ComputeManager.hh
index af80959..0f6bc62 100644
--- a/userFiles/ctrl/main/computation/ComputeManager.hh
+++ b/userFiles/ctrl/main/computation/ComputeManager.hh
@@ -15,7 +15,7 @@
 #include "CtrlOptions.hh"
 #include "MotorCtrlIndex.hh"
 
-enum{KINEMATICS_COMPUTE, ODOMETRY_COMPUTE, HANDLE_INPUTS, WALK_STATES, WHOLE_BODY, UPPER_BODY, IMPEDANCE_CTRL, HANDLE_OUTPUTS, MANAGER_NB};
+enum{KINEMATICS_COMPUTE, ODOMETRY_COMPUTE, HANDLE_INPUTS, WALK_STATES, WHOLE_BODY, UPPER_BODY, IMPEDANCE_CTRL, HANDLE_OUTPUTS, SWITCH_DATA, MANAGER_NB};
 
 /*! \brief Main manager calling the computation modules
  */
@@ -29,6 +29,10 @@ class ComputeManager
 
 		Computation* GetComputation(int index) { return compute_tab[inv_active_mod[index]]; }
 
+		void ResetCtrl();
+
+		CtrlInputs* Inputs() { return inputs; }
+
 	private:
 		std::vector<Computation*> compute_tab; ///< computation tabular
 		
diff --git a/userFiles/ctrl/parallel/SwitchData.cc b/userFiles/ctrl/parallel/SwitchData.cc
new file mode 100644
index 0000000..b024de9
--- /dev/null
+++ b/userFiles/ctrl/parallel/SwitchData.cc
@@ -0,0 +1,153 @@
+#include "SwitchData.hh"
+#include "BodyPart.hh"
+#include "cmake_config.h"
+#include "user_all_id.h"
+#include "coman_properties.hh"
+
+/*! \brief constructor
+ */
+SwitchData::SwitchData(CtrlInputs *inputs, CtrlOptions *options, MotorCtrlIndex *ctrl_index,
+	WalkStates* walk_states, WholeBody *whole_body):
+	Computation(inputs, options, ctrl_index)
+{
+	// safety
+	#ifdef PARALLEL_SIMU
+	std::cout << "SwitchData error: this file should not be called by a parallel simulation !" << std::endl;
+	exit(EXIT_FAILURE);
+	#endif
+
+	this->walk_states = walk_states;
+	this->whole_body  = whole_body;
+
+	sw_st     = static_cast<SwingStanceState*>(walk_states->State(SWING_STANCE_STATE));
+	stim_ctrl = static_cast<StimWalkCtrl*>(whole_body->StimCtrl());
+	matsu     = static_cast<Matsuoka2D*>(stim_ctrl->Osc());
+
+	nb_switchs = 0;
+	t_take_off = 0.0;
+
+	flag_switch   = false;
+	flag_foot_off = false;
+}
+
+/*! \brief destructor
+ */
+SwitchData::~SwitchData()
+{
+
+}
+
+/*! \brief main computation
+ */
+void SwitchData::Compute()
+{
+	// switch will be soon activated
+	if (flag_switch)
+	{
+		return;
+	}
+
+	// switch already performed
+	if (nb_switchs > 0)
+	{
+		return;
+	}
+
+	// enough number of strikes
+	if (!flag_foot_off && sw_st->NbStrikes() > 14)
+	{
+		// foot-off detected
+		if (sw_st->SwingLeg(R_ID) || sw_st->SwingLeg(L_ID))
+		{
+			flag_foot_off = true;
+			t_take_off = inputs->T();
+		}
+	}
+
+	// enough time after foot-off
+	if (flag_foot_off && inputs->T() - t_take_off > TIME_SWITCH_TAKE_OFF)
+	{
+		flag_switch   = true;
+		flag_foot_off = false;
+	}
+}
+
+/*! \brief write the robot current state
+ */
+void SwitchData::Data(std::vector<double> &data_vec)
+{
+	// variable declaration
+	BodyPart *cur_part;
+
+	// clear vector
+	data_vec.clear();
+	
+	// joint positions (q)
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		data_vec.push_back(inputs->Q(i));
+	}
+
+	// joint derivatives (qd)
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		data_vec.push_back(inputs->Qd(i));
+	}
+
+	// motor states and derivatives (ux & uxd)
+	for(int i=0; i<NB_MOTORS; i++)
+	{	
+		data_vec.push_back(inputs->QMot(i));
+	}
+
+	for(int i=0; i<NB_MOTORS; i++)
+	{
+		data_vec.push_back(inputs->QdMot(i));
+	}
+
+	// time
+	data_vec.push_back(inputs->T());
+
+	// swing legs
+	data_vec.push_back(sw_st->SwingLeg(0));
+	data_vec.push_back(sw_st->SwingLeg(1));
+
+	// supporting leg
+	data_vec.push_back(sw_st->SupportingRLeg());
+
+	// muscle lengths
+
+	for(int i=0; i<NB_BODY_PARTS; i++)
+	{
+		cur_part = whole_body->GetPart(i);
+
+		for(int j=0; j<cur_part->NbMuscles(); j++)
+		{
+			data_vec.push_back(cur_part->GetMuscle(j)->Lce());
+		}
+	}
+
+	// cpg
+	for(int i=0; i<matsu->NbNeurons(); i++)
+	{
+		data_vec.push_back(matsu->X(i));
+	}
+
+	for(int i=0; i<matsu->NbNeurons(); i++)
+	{
+		data_vec.push_back(matsu->V(i));
+	}
+
+	// remove flag switch
+	flag_switch = false;
+
+	// one extra switch performed
+	nb_switchs++;
+}
+
+/*! \brief reset the controller
+ */
+void SwitchData::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/parallel/SwitchData.hh b/userFiles/ctrl/parallel/SwitchData.hh
new file mode 100644
index 0000000..c26dca5
--- /dev/null
+++ b/userFiles/ctrl/parallel/SwitchData.hh
@@ -0,0 +1,44 @@
+/*! 
+ * \author Nicolas Van der Noot
+ * \file SwitchData.hh
+ * \brief SwitchData class
+ */
+
+#include "Computation.hh"
+#include "WalkStates.hh"
+#include "WholeBody.hh"
+#include "SwingStanceState.hh"
+#include "StimWalkCtrl.hh"
+#include "Matsuoka2D.hh"
+#include <vector>
+
+/*! \brief data to send to a parallel simulation when switchin
+ */
+class SwitchData: public Computation
+{
+	public:
+		SwitchData(CtrlInputs *inputs, CtrlOptions *options, MotorCtrlIndex *ctrl_index,
+			WalkStates* walk_states, WholeBody *whole_body);
+		virtual ~SwitchData();
+
+		virtual void Compute();
+		virtual void ResetCtrl();
+
+		bool FlagSwitch() const { return flag_switch; }
+
+		void Data(std::vector<double> &data_vec);
+
+	private:
+		WalkStates* walk_states; ///< walking states
+		WholeBody *whole_body;   ///< whole body control
+		SwingStanceState *sw_st; ///< swing-stance state
+		StimWalkCtrl *stim_ctrl; ///< stimulation controller
+		Matsuoka2D *matsu;       ///< matsuoka CPG
+
+		bool flag_switch;   ///< true if switch can be activated, false otherwise
+		bool flag_foot_off; ///< true if next switch can be launched (after foot-off), false otherwise
+
+		int nb_switchs;    ///< number of switchs performed
+		double t_take_off; ///< time last take-off was measured, when activating 'flag_foot_off' [s]
+};
+
diff --git a/userFiles/ctrl/states/WalkStates.cc b/userFiles/ctrl/states/WalkStates.cc
index ef54d79..b1737ee 100644
--- a/userFiles/ctrl/states/WalkStates.cc
+++ b/userFiles/ctrl/states/WalkStates.cc
@@ -88,3 +88,13 @@ void WalkStates::Compute()
 		states[i]->Compute();
 	}
 }
+
+/*! \brief reset the control saved variables
+ */
+void WalkStates::ResetCtrl()
+{
+	for(unsigned int i=0; i<states.size(); i++)
+	{
+		states[i]->ResetCtrl();
+	}
+}
diff --git a/userFiles/ctrl/states/WalkStates.hh b/userFiles/ctrl/states/WalkStates.hh
index d938c81..7bdfc86 100644
--- a/userFiles/ctrl/states/WalkStates.hh
+++ b/userFiles/ctrl/states/WalkStates.hh
@@ -22,6 +22,7 @@ class WalkStates: public Computation
 		virtual ~WalkStates();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 		WalkState* State(int i) const { return states[i]; }
 
diff --git a/userFiles/ctrl/states/generic/WalkState.hh b/userFiles/ctrl/states/generic/WalkState.hh
index b8a8f19..3730ff1 100644
--- a/userFiles/ctrl/states/generic/WalkState.hh
+++ b/userFiles/ctrl/states/generic/WalkState.hh
@@ -20,6 +20,7 @@ class WalkState
 		virtual ~WalkState();
 
 		virtual void Compute() = 0;
+		virtual void ResetCtrl() = 0;
 
 	protected:
 		CtrlInputs *inputs; ///< controller inputs
diff --git a/userFiles/ctrl/states/specific_states/MainState.cc b/userFiles/ctrl/states/specific_states/MainState.cc
index ce7fb16..d474384 100644
--- a/userFiles/ctrl/states/specific_states/MainState.cc
+++ b/userFiles/ctrl/states/specific_states/MainState.cc
@@ -55,3 +55,10 @@ void MainState::Compute()
 			exit(EXIT_FAILURE);
 	}
 }
+
+/*! \brief reset the controller
+ */
+void MainState::ResetCtrl()
+{
+	coman_state = (t >= t_walk_start) ? WALK_COMAN_STATE : INIT_UPRIGHT_STATE;
+}
diff --git a/userFiles/ctrl/states/specific_states/MainState.hh b/userFiles/ctrl/states/specific_states/MainState.hh
index 5638966..faacfc2 100644
--- a/userFiles/ctrl/states/specific_states/MainState.hh
+++ b/userFiles/ctrl/states/specific_states/MainState.hh
@@ -27,6 +27,8 @@ class MainState: public WalkState
 
 		double InitTWalk() const { return init_t_walk; }
 
+		virtual void ResetCtrl();
+
 	private:
 		double t; ///< time [s]
 
diff --git a/userFiles/ctrl/states/specific_states/SwingStanceState.cc b/userFiles/ctrl/states/specific_states/SwingStanceState.cc
index c5f72c7..3559921 100644
--- a/userFiles/ctrl/states/specific_states/SwingStanceState.cc
+++ b/userFiles/ctrl/states/specific_states/SwingStanceState.cc
@@ -1,4 +1,5 @@
 #include "SwingStanceState.hh"
+#include "coman_properties.hh"
 
 #define FOOT_THRESHOLD 20.0 // [N]
 #define FOOT_THRESHOLD_INIT 50.0 // [N]
@@ -9,6 +10,8 @@
 
 #define THRESHOLD_FOOT_ABS 0.6 // [rad]
 
+#define MEAN_TIME_STEP 0.5 // [s]
+
 /*! \brief constructor
  */
 SwingStanceState::SwingStanceState(CtrlInputs *inputs, CtrlOptions *options, MainState *main_state): WalkState(inputs, options)
@@ -147,3 +150,64 @@ int SwingStanceState::PossibleSwing(int leg_id)
 
 	return ( (inputs->MeanFzFeet(leg_id) < foot_threshold) && (inputs->FzFeet(leg_id) < foot_threshold) );
 }
+
+/*! \brief reset the controller
+ */
+void SwingStanceState::ResetCtrl()
+{
+	flag_strike = 0;
+
+	for(int i=0; i<NB_LEGS; i++)
+	{
+		flag_strike_leg[i] = 0;
+	}
+}
+
+/*! \brief reset the controller, with states into account
+ */
+void SwingStanceState::ResetCtrlState()
+{
+	ResetCtrl();
+
+	t_start_last_swing = inputs->T() - TIME_SWITCH_TAKE_OFF;
+
+	t_start_last_stance = (swing_leg[R_ID] || swing_leg[L_ID]) ? t_start_last_swing - 0.1 : inputs->T();
+
+	if (supporting_r_leg)
+	{
+		t_start_last_swing_leg[L_ID] = t_start_last_swing;
+		t_start_last_swing_leg[R_ID] = t_start_last_swing - MEAN_TIME_STEP;
+
+		if (swing_leg[R_ID] || swing_leg[L_ID])
+		{
+			t_start_last_stance_leg[R_ID] = t_start_last_stance;
+			t_start_last_stance_leg[L_ID] = t_start_last_stance - MEAN_TIME_STEP;
+		}
+		else
+		{
+			t_start_last_stance_leg[L_ID] = t_start_last_stance;
+			t_start_last_stance_leg[R_ID] = t_start_last_stance - MEAN_TIME_STEP;
+		}
+	}
+	else
+	{
+		t_start_last_swing_leg[R_ID] = t_start_last_swing;
+		t_start_last_swing_leg[L_ID] = t_start_last_swing - MEAN_TIME_STEP;
+
+		if (swing_leg[R_ID] || swing_leg[L_ID])
+		{
+			t_start_last_stance_leg[L_ID] = t_start_last_stance;
+			t_start_last_stance_leg[R_ID] = t_start_last_stance - MEAN_TIME_STEP;
+		}
+		else
+		{
+			t_start_last_stance_leg[R_ID] = t_start_last_stance;
+			t_start_last_stance_leg[L_ID] = t_start_last_stance - MEAN_TIME_STEP;
+		}
+	}
+
+	for(int i=0; i<NB_LEGS; i++)
+	{
+		last_t_stance[i] = t_start_last_swing_leg[i];
+	}
+}
diff --git a/userFiles/ctrl/states/specific_states/SwingStanceState.hh b/userFiles/ctrl/states/specific_states/SwingStanceState.hh
index 5dfadbd..433a20e 100644
--- a/userFiles/ctrl/states/specific_states/SwingStanceState.hh
+++ b/userFiles/ctrl/states/specific_states/SwingStanceState.hh
@@ -30,6 +30,14 @@ class SwingStanceState: public WalkState
 
 		double TStartLastStance() const { return t_start_last_stance; }
 
+		virtual void ResetCtrl();
+
+		void ResetCtrlState();
+
+		void SetSwingLeg(int i, int value) { swing_leg[i] = value; }
+
+		void SetSupportingRLeg(int value) { supporting_r_leg = value; }
+
 	private:
 		int nb_strikes; ///< number of feet strikes
 
diff --git a/userFiles/ctrl/states/specific_states/TrailingState.cc b/userFiles/ctrl/states/specific_states/TrailingState.cc
index ff4e0d5..f9acfed 100644
--- a/userFiles/ctrl/states/specific_states/TrailingState.cc
+++ b/userFiles/ctrl/states/specific_states/TrailingState.cc
@@ -85,3 +85,47 @@ void TrailingState::Compute()
 		last_r_swing = (mean_Fz_R_foot > mean_Fz_L_foot) ? 0 : 1;
 	}	
 }
+
+/*! \brief reset the controller
+ */
+void TrailingState::ResetCtrl()
+{
+
+}
+
+/*! \brief reset the controller, with states into account
+ */
+void TrailingState::ResetCtrlState()
+{
+	// suppose: not double support
+	for(int i=0; i<NB_LEGS; i++)
+	{
+		trailing_leg[i] = 0;
+	}
+	trailing_state = ONE_IN_SWING;
+
+
+	if (sw_st->SwingLeg(R_ID)) // right swing
+	{
+		last_r_swing = 1;
+	}
+	else if (sw_st->SwingLeg(L_ID)) // left swing
+	{
+		last_r_swing = 0;
+	}
+	else // double support
+	{
+		if (sw_st->SupportingRLeg())
+		{
+			trailing_leg[L_ID] = 1;
+			last_r_swing = 1;
+		}
+		else
+		{
+			trailing_leg[R_ID] = 1;
+			last_r_swing = 0;
+		}
+
+		trailing_state = BEFORE_TAKE_OFF;
+	}
+}
diff --git a/userFiles/ctrl/states/specific_states/TrailingState.hh b/userFiles/ctrl/states/specific_states/TrailingState.hh
index 16fe3a5..3531e67 100644
--- a/userFiles/ctrl/states/specific_states/TrailingState.hh
+++ b/userFiles/ctrl/states/specific_states/TrailingState.hh
@@ -19,9 +19,14 @@ class TrailingState: public WalkState
 		virtual ~TrailingState();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
+
+		void ResetCtrlState();
 
 		int TrailingLeg(int i) const { return trailing_leg[i]; }
 
+		void SetTrailingLeg(int i, int value) { trailing_leg[i] = value; }
+
 	private:
 		int last_r_swing;   ///< 1 if last leg in swing is the right one
 		int trailing_state; ///< general state of the COMAN according to the trailing legs
diff --git a/userFiles/ctrl/upper/UpperBody.cc b/userFiles/ctrl/upper/UpperBody.cc
index 2cc13d8..78d7b5a 100644
--- a/userFiles/ctrl/upper/UpperBody.cc
+++ b/userFiles/ctrl/upper/UpperBody.cc
@@ -123,3 +123,10 @@ void UpperBody::Compute()
 		outputs->SetQRef(LeftElbPitch_id, FIXED_ELBOW_REF);
 	}
 }
+
+/*! \brief reset the control saved variables
+ */
+void UpperBody::ResetCtrl()
+{
+
+}
diff --git a/userFiles/ctrl/upper/UpperBody.hh b/userFiles/ctrl/upper/UpperBody.hh
index 0aac0bb..8160f4c 100644
--- a/userFiles/ctrl/upper/UpperBody.hh
+++ b/userFiles/ctrl/upper/UpperBody.hh
@@ -23,6 +23,7 @@ class UpperBody: public Computation
 		virtual ~UpperBody();
 
 		virtual void Compute();
+		virtual void ResetCtrl();
 
 	private:
 		CtrlOutputs *outputs; ///< controller outputs
diff --git a/userFiles/ctrl/useful/delay/Delay.cc b/userFiles/ctrl/useful/delay/Delay.cc
index 5cd69d2..2eab4ba 100644
--- a/userFiles/ctrl/useful/delay/Delay.cc
+++ b/userFiles/ctrl/useful/delay/Delay.cc
@@ -93,3 +93,19 @@ double Delay::ValueDelay(double cur_t)
 	// return last element added
 	return vec_val[last_index];
 }
+
+/*! \brief reset the delay manager
+ * 
+ * \param[in] cur_t current time [s]
+ * \param[in] cur_val current value
+ */
+void Delay::Reset(double cur_t, double cur_val)
+{
+	index = 0;
+
+	for(int i=0; i<size_vec; i++)
+	{
+		vec_t[i]   = cur_t;
+		vec_val[i] = cur_val;
+	}
+}
diff --git a/userFiles/ctrl/useful/delay/Delay.hh b/userFiles/ctrl/useful/delay/Delay.hh
index 21f61e5..1bd00aa 100644
--- a/userFiles/ctrl/useful/delay/Delay.hh
+++ b/userFiles/ctrl/useful/delay/Delay.hh
@@ -18,6 +18,8 @@ class Delay
 		void NewValueTime(double new_t, double new_val);
 		double ValueDelay(double cur_t);
 
+		void Reset(double cur_t, double cur_val);
+
 		/*! \brief add a new value to delay and get the current delayed value
 		 * 
 		 * \param[in] cur_t current time (time of the new value) [s]
diff --git a/userFiles/ctrl/useful/low_filter/LowFilter.cc b/userFiles/ctrl/useful/low_filter/LowFilter.cc
index 65e5221..a6be5e9 100644
--- a/userFiles/ctrl/useful/low_filter/LowFilter.cc
+++ b/userFiles/ctrl/useful/low_filter/LowFilter.cc
@@ -52,3 +52,15 @@ void LowFilter::Update(double x, double cur_t)
 
 	y = f * frac * x + frac * y;
 }
+
+/*! \brief reset the low-pass filter
+ * 
+ * \param[in] x new value
+ * \param[in] cur_r current time [s]
+ */
+void LowFilter::Reset(double x, double cur_t)
+{
+	y = x;
+
+	last_t = cur_t;
+}
diff --git a/userFiles/ctrl/useful/low_filter/LowFilter.hh b/userFiles/ctrl/useful/low_filter/LowFilter.hh
index a826a0c..ea46e16 100644
--- a/userFiles/ctrl/useful/low_filter/LowFilter.hh
+++ b/userFiles/ctrl/useful/low_filter/LowFilter.hh
@@ -39,6 +39,8 @@ class LowFilter
 			return Get();
 		}
 
+		void Reset(double x, double cur_t);
+
 	private:
 		double y;      ///< output value
 		double last_t; ///< last time value added
diff --git a/userFiles/ctrl/useful/noise/Noise.cc b/userFiles/ctrl/useful/noise/Noise.cc
index 9d0e869..4df1609 100644
--- a/userFiles/ctrl/useful/noise/Noise.cc
+++ b/userFiles/ctrl/useful/noise/Noise.cc
@@ -36,16 +36,29 @@ void Noise::Update(double t)
 
 	// new random value generated
 	if(diff_t >= t_period)
-    {
-    	cur_n  = next_n;
-    	last_n = next_n;
-        next_n = NewNoise();
-
-        last_t = t;
-    }
-    // interpolation between two random values
-    else
-    {
-    	cur_n = (next_n - last_n) * (diff_t / t_period) + last_n;
-    }
+	{
+		cur_n  = next_n;
+		last_n = next_n;
+		next_n = NewNoise();
+
+		last_t = t;
+	}
+	// interpolation between two random values
+	else
+	{
+		cur_n = (next_n - last_n) * (diff_t / t_period) + last_n;
+	}
+}
+
+/*! \brief reset noise computation
+ * 
+ * \param[in] t current time [s]
+ */
+void Noise::Reset(double t)
+{
+	last_t = t;
+
+	last_n = NewNoise();
+	next_n = NewNoise();
+	cur_n  = last_n;
 }
diff --git a/userFiles/ctrl/useful/noise/Noise.hh b/userFiles/ctrl/useful/noise/Noise.hh
index a5facfd..1d55579 100644
--- a/userFiles/ctrl/useful/noise/Noise.hh
+++ b/userFiles/ctrl/useful/noise/Noise.hh
@@ -19,6 +19,8 @@ class Noise
 
 		void Update(double t);
 
+		void Reset(double t);
+
 		/*! \brief get current noise level
 		 * 
 		 * \return current noise value, bounded in [-max_noise ; max_noise]
diff --git a/userFiles/ctrl/useful/running_average/RunningAverage.cc b/userFiles/ctrl/useful/running_average/RunningAverage.cc
index 5f40d95..a421cf5 100644
--- a/userFiles/ctrl/useful/running_average/RunningAverage.cc
+++ b/userFiles/ctrl/useful/running_average/RunningAverage.cc
@@ -48,3 +48,19 @@ void RunningAverage::NewValue(double new_val)
 		index = 0;
 	}
 }
+
+/*! \brief reset the running average so that it is full of a single new value
+ * 
+ * \param[in] new_val new value to apply to the whole filter
+ */
+void RunningAverage::Reset(double new_val)
+{
+	for(int i=0; i<size_vec; i++)
+	{
+		vec[i] = new_val;
+	}
+
+	index = 0;
+
+	sum = new_val * size_vec;
+}
diff --git a/userFiles/ctrl/useful/running_average/RunningAverage.hh b/userFiles/ctrl/useful/running_average/RunningAverage.hh
index 7a0110f..9ae151f 100644
--- a/userFiles/ctrl/useful/running_average/RunningAverage.hh
+++ b/userFiles/ctrl/useful/running_average/RunningAverage.hh
@@ -17,6 +17,8 @@ class RunningAverage
 
 		void NewValue(double new_val);
 
+		void Reset(double new_val);
+
 		/*! \brief get the current average
 		 * 
 		 * \return current average
diff --git a/userFiles/interface/main/Ctrl.hh b/userFiles/interface/main/Ctrl.hh
index fcb8863..8aefd13 100644
--- a/userFiles/interface/main/Ctrl.hh
+++ b/userFiles/interface/main/Ctrl.hh
@@ -11,10 +11,6 @@
 #include "UserCtrl.hh"
 #include <iostream>
 
-#ifdef ROBOTRAN_SIMU
-	#include "user_realtime.h"
-#endif
-
 enum{BASIC_CTRL, EXAMPLE_CTRL, NICO_CTRL, SALMAN_CTRL};
 
 /*! \brief Main controller
diff --git a/userFiles/interface/user/UserCtrl.cc b/userFiles/interface/user/UserCtrl.cc
index 8e51cfb..5242586 100644
--- a/userFiles/interface/user/UserCtrl.cc
+++ b/userFiles/interface/user/UserCtrl.cc
@@ -67,7 +67,7 @@ UserCtrl::UserCtrl()
 	// target forward speed
 	v_fwd = 0.65;
 	s_length = 0.40;
-	s_height = 0.065;
+	s_height = 0.06; // TEMPORARY: 0.065 before
 
 	for(int j=0; j<10; j++)
 	{
diff --git a/userFiles/simu/actuators/Actuator.cc b/userFiles/simu/actuators/Actuator.cc
index 5060ab1..99aaaf4 100644
--- a/userFiles/simu/actuators/Actuator.cc
+++ b/userFiles/simu/actuators/Actuator.cc
@@ -189,3 +189,11 @@ void Actuator::LowLevelCtrl()
 
 	u = limit(u, -MAX_ACT_VOLTAGE, MAX_ACT_VOLTAGE);
 }
+
+/*! \brief reset the integral term
+ */
+void Actuator::ResetInt()
+{
+	torque_noise->Reset(mbs_data->tsim);
+	int_err_Qq = 0.0;
+}
diff --git a/userFiles/simu/actuators/Actuator.hh b/userFiles/simu/actuators/Actuator.hh
index c548d94..76995ed 100644
--- a/userFiles/simu/actuators/Actuator.hh
+++ b/userFiles/simu/actuators/Actuator.hh
@@ -27,6 +27,8 @@ class Actuator
 		void compute_acceleration();
 		void compute_torque();
 
+		void ResetInt();
+
 		double QddM() const { return qdd_m; }
 		double QQ() const { return Qq; }
 		double QqRef() const { return Qq_ref; }
diff --git a/userFiles/simu/actuators/RobotActuators.cc b/userFiles/simu/actuators/RobotActuators.cc
index 0cc1d4b..ba3526e 100644
--- a/userFiles/simu/actuators/RobotActuators.cc
+++ b/userFiles/simu/actuators/RobotActuators.cc
@@ -165,3 +165,13 @@ void RobotActuators::MotorComputeTor(double *ux, double *uxd, double *Qq)
 		Qq[cur_act->MbsIndex()] = (cur_act->PerfectTorFlag()) ? cur_act->QqRef() : cur_act->QQ();
 	}
 }
+
+/*! \brief reset the integral term of the actuators
+ */
+void RobotActuators::ResetInt()
+{
+	for(int i=0; i<nb_mot; i++)
+	{
+		actuators_list[i]->ResetInt();
+	}
+}
diff --git a/userFiles/simu/actuators/RobotActuators.hh b/userFiles/simu/actuators/RobotActuators.hh
index 96ded58..5f972d1 100644
--- a/userFiles/simu/actuators/RobotActuators.hh
+++ b/userFiles/simu/actuators/RobotActuators.hh
@@ -25,6 +25,8 @@ class RobotActuators
 
 		int NbActuators() const { return actuators_list.size(); }
 
+		void ResetInt();
+
 		int NbMot() const { return nb_mot; }
 
 		int InvActIndex(int i) const { return inv_actuators_index[i]; }
diff --git a/userFiles/simu/analysis/features/specific_features/generic/FeatureAnalysis.hh b/userFiles/simu/analysis/features/specific_features/generic/FeatureAnalysis.hh
index cdb8a82..88d061a 100644
--- a/userFiles/simu/analysis/features/specific_features/generic/FeatureAnalysis.hh
+++ b/userFiles/simu/analysis/features/specific_features/generic/FeatureAnalysis.hh
@@ -7,7 +7,6 @@
 #ifndef _FEATURE_ANALYSIS_HH_
 #define _FEATURE_ANALYSIS_HH_
 
-#include "user_realtime.h"
 #include "mbs_data.h"
 #include "ModelSimuIndex.hh"
 
diff --git a/userFiles/simu/analysis/features/specific_features/swing_stance/SwingStanceAnalysis.cc b/userFiles/simu/analysis/features/specific_features/swing_stance/SwingStanceAnalysis.cc
index 578262e..e48792e 100644
--- a/userFiles/simu/analysis/features/specific_features/swing_stance/SwingStanceAnalysis.cc
+++ b/userFiles/simu/analysis/features/specific_features/swing_stance/SwingStanceAnalysis.cc
@@ -178,8 +178,8 @@ void SwingStanceAnalysis::Compute()
 			{
 				step_height = (sens_info->SRFootP(2) > sens_info->SLFootP(2))? sens_info->SRFootP(2) - FOOT_LENGTH_FWD*sin(sens_info->ThetaRFoot(1)) : sens_info->SLFootP(2) - FOOT_LENGTH_FWD*sin(sens_info->ThetaLFoot(1));
 				
-				set_plot(step_height, "step height");
-				set_output(step_height, "sh");
+				//set_plot(step_height, "step height");
+				//set_output(step_height, "sh");
 				if((MIN_T_MEAN < t) && (t < MAX_T_MEAN))
 				{
 					step_height_mean = stp_height_av->UpdateGet(step_height);
@@ -223,8 +223,8 @@ void SwingStanceAnalysis::Compute()
 				step_length      = x_foot - x_last_strike[!i]; //norm2D(x_foot - x_last_strike[!i], y_foot - y_last_strike[!i]);
 				stride_length[i] = norm2D(x_foot - x_last_strike[i] , y_foot - y_last_strike[i]);
 
-				set_plot(step_length, "step length");
-				set_output(step_length, "sl");
+				//set_plot(step_length, "step length");
+				//set_output(step_length, "sl");
 
 				lat_foot_dist = DistPointLine(x_last_strike[!i], y_last_strike[!i], x_last_strike[i], y_last_strike[i], x_foot, y_foot);
 
@@ -294,6 +294,7 @@ void SwingStanceAnalysis::Compute()
 		}
 	}
 
+	/*
 	if (options->print && !flag_print && mbs_data->tsim > MID_STOP_MEAN)
 	{
 		flag_print = 1;
@@ -309,7 +310,7 @@ void SwingStanceAnalysis::Compute()
 		std::cout << "step period 2: " << stride_period_mean/2.0 << " [s]" << std::endl;
 		std::cout << "step length 2: " << stride_length_mean/2.0 << " [m]" << std::endl;
 		std::cout << "step height 2: " << step_height_mean   << " [m]" << std::endl;
-	}
+	}//*/
 
 }
 
diff --git a/userFiles/simu/analysis/fitness/specific_fitness/steering/SteeringFitness.cc b/userFiles/simu/analysis/fitness/specific_fitness/steering/SteeringFitness.cc
index 89090e7..df91da2 100644
--- a/userFiles/simu/analysis/fitness/specific_fitness/steering/SteeringFitness.cc
+++ b/userFiles/simu/analysis/fitness/specific_fitness/steering/SteeringFitness.cc
@@ -1,6 +1,8 @@
 #include "SteeringFitness.hh"
 #include "NicoCtrl.hh"
 #include "WholeBody.hh"
+#include "user_realtime.h"
+
 #include <stdlib.h>
 #include <iostream>
 
diff --git a/userFiles/simu/forces/meshGCM/main/ComputeGCM.cc b/userFiles/simu/forces/meshGCM/main/ComputeGCM.cc
index ca62339..cf2a5e0 100644
--- a/userFiles/simu/forces/meshGCM/main/ComputeGCM.cc
+++ b/userFiles/simu/forces/meshGCM/main/ComputeGCM.cc
@@ -13,6 +13,7 @@
 
 #include "user_IO.h"
 #include "set_output.h"
+#include "user_realtime.h"
 
 /*! \brief constructor
  * 
@@ -133,3 +134,10 @@ void ComputeGCM::CopCompute(int nb_feet)
 	set_plot(x_cop, "COP x");
 	set_plot(y_cop, "COP y");
 }
+
+/*! \brief reset the GCM
+ */
+void ComputeGCM::ResetGCM()
+{
+	whole_feet->ResetGCM();
+}
diff --git a/userFiles/simu/forces/meshGCM/main/ComputeGCM.hh b/userFiles/simu/forces/meshGCM/main/ComputeGCM.hh
index b5b3334..3baf980 100644
--- a/userFiles/simu/forces/meshGCM/main/ComputeGCM.hh
+++ b/userFiles/simu/forces/meshGCM/main/ComputeGCM.hh
@@ -34,6 +34,10 @@ class ComputeGCM
 		/// get the norm of the total force for one body
 		double FNorm(int body_id) { return whole_feet->FootBody(body_id)->FNorm(); }
 
+		void ResetGCM();
+
+		GroundModel* Ground() { return ground; }
+
 	private:
 		int coman_model; ///< COMAN model
 
diff --git a/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.cc b/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.cc
index 7411c89..5cf63d9 100644
--- a/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.cc
+++ b/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.cc
@@ -159,3 +159,34 @@ void ContactFoot::UpdateLowPass(double new_t)
 		low_filters[i]->UpdateParams(mbs_data->tsim);
 	}
 }
+
+/*! \brief reset the GCM
+ */
+void ContactFoot::ResetGCM()
+{
+	double tsim;
+
+	
+	for (int i=0; i<nb_points; i++)
+	{
+		for (int j=0; j<3; j++)
+		{
+			rs[i][j] = 0.0;
+		}
+
+		state[i] = SLIDING_GCM;
+
+		Fn_lp[i] = 0.0;
+		fx_lp[i] = 0.0;
+		fy_lp[i] = 0.0;
+		vx_lp[i] = 0.0;
+		vy_lp[i] = 0.0;//*/
+	}
+
+	tsim = mbs_data->tsim;
+
+	for(int i=0; i<NB_LOW_FILT_MESH; i++)
+	{
+		low_filters[i]->Reset(tsim);
+	}
+}
diff --git a/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.hh b/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.hh
index e15dc56..e1a09c8 100644
--- a/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.hh
+++ b/userFiles/simu/forces/meshGCM/specific_foot/generic/ContactFoot.hh
@@ -69,6 +69,8 @@ class ContactFoot
 		void SetTTot(int id_axis, double value) { T_tot[id_axis] = value; }
 		void SetRCop(int id_axis, double value) { r_COP[id_axis] = value; }
 
+		void ResetGCM();
+
 	protected:
 		MbsData *mbs_data;           ///< Robotran structure
 		GaitFeatures *gait_features; ///< gait features class
diff --git a/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.cc b/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.cc
index a3d348b..334c8cb 100644
--- a/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.cc
+++ b/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.cc
@@ -36,3 +36,13 @@ void WholeFeet::StateCompute()
 		ground->StateSwitch(bodies[i]);
 	}
 }
+
+/*! \brief reset the GCM
+ */
+void WholeFeet::ResetGCM()
+{
+	for(unsigned int i=0; i<bodies.size(); i++)
+	{
+		bodies[i]->ResetGCM();
+	}
+}
diff --git a/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.hh b/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.hh
index cdaf3a8..c019fa7 100644
--- a/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.hh
+++ b/userFiles/simu/forces/meshGCM/specific_whole_feet/generic/WholeFeet.hh
@@ -36,6 +36,8 @@ class WholeFeet
 		/// get contact foot corresponding to the provided ID
 		ContactFoot* FootBody(int p) { return bodies[p]; }
 
+		void ResetGCM();
+
 	protected:
 		std::vector<ContactFoot*> bodies; ///< bodies of the feet
 
diff --git a/userFiles/simu/io/sensors/SensorsInfo.cc b/userFiles/simu/io/sensors/SensorsInfo.cc
index e17ce40..7750fba 100644
--- a/userFiles/simu/io/sensors/SensorsInfo.cc
+++ b/userFiles/simu/io/sensors/SensorsInfo.cc
@@ -12,6 +12,8 @@ extern "C"
 #include "user_IO.h"
 #include "set_output.h"
 
+#include "user_realtime.h"
+
 /// square
 inline double square(double x) { return x*x; }
 
diff --git a/userFiles/simu/io/simu_options/SimuOptions.c b/userFiles/simu/io/simu_options/SimuOptions.c
index 5ffd79a..7621cd6 100644
--- a/userFiles/simu/io/simu_options/SimuOptions.c
+++ b/userFiles/simu/io/simu_options/SimuOptions.c
@@ -36,7 +36,7 @@ SimuOptions* init_SimuOptions()
 	options->stepHeightLength_opti = 1;
 
 	// 1 if model can be stopped, 0 otherwise
-	options->stop = 1;
+	options->stop = 0; // TEMPORARY
 
 	// 1 to use the body contact model, 0 otherwise
 	options->body_contact = 0;
@@ -57,7 +57,7 @@ SimuOptions* init_SimuOptions()
 	options->opti_length_ref = 0.4;
 
 	// 1 to add noise on the torque readings, 0 otherwise
-	options->torque_noise = 1;
+	options->torque_noise = 0; // TEMPORARY
 
 	// 1 to print comments, 0 otherwise
 	#if defined(EVAL_RUN) || defined(OPTI_BIOROB) || defined(OPTI_MPI)
diff --git a/userFiles/simu/io/simu_options/SimuOptions.h b/userFiles/simu/io/simu_options/SimuOptions.h
index 43b4bd1..d21591b 100644
--- a/userFiles/simu/io/simu_options/SimuOptions.h
+++ b/userFiles/simu/io/simu_options/SimuOptions.h
@@ -6,8 +6,6 @@
 #ifndef _SIMU_OPTIONS_H_
 #define _SIMU_OPTIONS_H_
 
-#include "user_realtime.h"
-
 // COMAN version
 enum {INIT_FEET_COMAN, SHORT_FEET_COMAN, LONG_FEET_COMAN, FLEX_FEET_COMAN};
 
diff --git a/userFiles/simu/main/SimuCtrl.cc b/userFiles/simu/main/SimuCtrl.cc
index dfe6355..259c9b5 100644
--- a/userFiles/simu/main/SimuCtrl.cc
+++ b/userFiles/simu/main/SimuCtrl.cc
@@ -10,6 +10,7 @@
 #include "SimuOptions.h"
 #include "ContactGestion.hh"
 #include "SimuIndex.hh"
+#include "MultiSimu.hh"
 
 #define PERIOD_CTRL 1.0e-3  ///< period of the controller [s]
 #define TIME_EPSILON 1.0e-5 ///< safety to check the period of the controller [s]
@@ -100,6 +101,12 @@ SimuCtrl::SimuCtrl(MbsData *mbs_data, Ctrl *ctrl)
 				compute_tab.push_back(new CameraGestion(mbs_data, simu_index));
 				break;
 
+			case MULTI_SIMU_GESTION:
+				#ifndef PARALLEL_SIMU
+				compute_tab.push_back(new MultiSimu(mbs_data, simu_index));
+				#endif
+				break;
+
 			default:
 				break;
 		}
diff --git a/userFiles/simu/main/SimuCtrl.hh b/userFiles/simu/main/SimuCtrl.hh
index 4389c89..260320e 100644
--- a/userFiles/simu/main/SimuCtrl.hh
+++ b/userFiles/simu/main/SimuCtrl.hh
@@ -8,7 +8,6 @@
 #define _SIMU_CTRL_HH_
 
 #include "mbs_data.h"
-#include "user_realtime.h"
 #include "Ctrl.hh"
 #include "ComputationSimu.hh"
 #include "ComputeGCM.hh"
@@ -23,7 +22,8 @@
 #include <iostream>
 
 /// simulation control structures
-enum{SENSORS_INFO, GAIT_FEATURES, STOP_SIMULATION, FITNESS, PUSH_TORSO, CAMERA_GESTION, SIMU_CTRL_NB};
+enum{SENSORS_INFO, GAIT_FEATURES, STOP_SIMULATION, FITNESS, PUSH_TORSO,
+	CAMERA_GESTION, MULTI_SIMU_GESTION, SIMU_CTRL_NB};
 
 /*! \brief Main class to handle the controller and the user simulation functions
  */
@@ -51,6 +51,8 @@ class SimuCtrl
 		bool PushFlag()   const { return push_flag;   }
 		bool CameraFlag() const { return camera_flag; }
 
+		void SetLastTCtrl(double val) { last_t_ctrl = val; }
+
 	private:
 		int gcm_model; ///< 1 for GCM mesh, 0 otherwise
 		double last_t_ctrl; ///< last time controller was called [s]
diff --git a/userFiles/simu/parallel/MultiSimu.cc b/userFiles/simu/parallel/MultiSimu.cc
new file mode 100644
index 0000000..3c350c2
--- /dev/null
+++ b/userFiles/simu/parallel/MultiSimu.cc
@@ -0,0 +1,174 @@
+#include "MultiSimu.hh"
+#include "cmake_config.h"
+
+#include "user_model.h"
+#include "user_all_id.h"
+#include "coman_properties.hh"
+
+#include "CppInterface.hh"
+#include "SimuCtrl.hh"
+#include "NicoCtrl.hh"
+#include "ComputeManager.hh"
+#include "SwitchData.hh"
+
+#include <fstream>
+
+// states from the controller
+#define NB_TIME 1     ///< number of time values
+#define NB_SW_ST 3    ///< number of swing-stance states
+#define NB_MUSCLES 52 ///< number of muscle lengths
+#define NB_NEURONS 10 ///< number of neurons
+
+#define NB_CTRL_DATA (4*NB_MOTORS + NB_TIME + NB_SW_ST + NB_MUSCLES + 2*NB_NEURONS) ///< number of controller state variables
+
+/*! \brief constructor
+ */
+MultiSimu::MultiSimu(MbsData *mbs_data, ModelSimuIndex *simu_index):
+	ComputationSimu(mbs_data, simu_index)
+{
+	// safety
+	#ifdef PARALLEL_SIMU
+	std::cout << "MultiSimu error: this file should not be called by a parallel simulation !" << std::endl;
+	exit(EXIT_FAILURE);
+	#endif
+
+	// find path of the configuration file
+	config_txt  = PROJECT_SOURCE_DIR;
+	config_txt += "/../parallel_simu/config/config.txt";
+
+	// find path of the start file
+	start_txt  = PROJECT_SOURCE_DIR;
+	start_txt += "/../parallel_simu/config/start.txt";
+
+	WriteStart();
+}
+
+/*! \brief destructor
+ */
+MultiSimu::~MultiSimu()
+{
+
+}
+
+/*! \brief main computation
+ */
+void MultiSimu::Compute()
+{
+	SimuCtrl *simu_ctrl;
+	NicoCtrl *ctrl;
+	SwitchData *switch_data;
+
+	// get controller fields
+	simu_ctrl   = static_cast<CppInterface*>(mbs_data->user_model->cpp.interface)->GetSimuCtrl();
+	ctrl        = static_cast<NicoCtrl*>(simu_ctrl->GetCtrl());
+	switch_data = static_cast<SwitchData*>(ctrl->Manager()->GetComputation(SWITCH_DATA));
+
+	if (switch_data->FlagSwitch())
+	{
+		WriteState();
+	}
+}
+
+/*! \brief write the start file
+ */
+void MultiSimu::WriteStart()
+{
+	std::ofstream start_str;
+
+	// remove old file
+	if (CheckFileExist(start_txt))
+	{
+		if( remove( start_txt.c_str() ) != 0 )
+		{
+			std::cout << "MultiSimu: error deleting " << start_txt << " file !" << std::endl;
+			exit(EXIT_FAILURE);
+		}
+	}
+
+	// create and open new file
+	start_str.open(start_txt.c_str());
+
+	// write anything
+	start_str << "start" << std::endl;
+
+	// close file
+	start_str.close();
+}
+
+/*! \brief write the robot current state
+ */
+void MultiSimu::WriteState()
+{
+	// variable declaration
+	std::ofstream config_str;
+
+	SimuCtrl *simu_ctrl;
+	NicoCtrl *ctrl;
+	SwitchData *switch_data;
+
+	// get simulation and controller fields
+	simu_ctrl   = static_cast<CppInterface*>(mbs_data->user_model->cpp.interface)->GetSimuCtrl();
+	ctrl        = static_cast<NicoCtrl*>(simu_ctrl->GetCtrl());
+	switch_data = static_cast<SwitchData*>(ctrl->Manager()->GetComputation(SWITCH_DATA));
+
+	// remove old file
+	if (CheckFileExist(config_txt))
+	{
+		if( remove( config_txt.c_str() ) != 0 )
+		{
+			std::cout << "MultiSimu: error deleting " << config_txt << " file !" << std::endl;
+			exit(EXIT_FAILURE);
+		}
+	}
+
+	// create and open new file
+	config_str.open(config_txt.c_str());
+
+	// floating base position
+	config_str << mbs_data->q[FJ_T1_Coman_id] << std::endl;
+	config_str << mbs_data->q[FJ_T3_Coman_id] << std::endl;
+	config_str << mbs_data->q[FJ_R2_Coman_id] << std::endl;
+
+	// floating base velocity
+	config_str << mbs_data->qd[FJ_T1_Coman_id] << std::endl;
+	config_str << mbs_data->qd[FJ_T3_Coman_id] << std::endl;
+	config_str << mbs_data->qd[FJ_R2_Coman_id] << std::endl;
+
+	// get data from controller
+	switch_data->Data(data_vec);
+
+	// safety
+	if (data_vec.size() != NB_CTRL_DATA)
+	{
+		std::cout << "MultiSimu error: data controller size incorrect: " << data_vec.size()
+			<< " >< " << NB_CTRL_DATA << " !" << std::endl;
+		exit(EXIT_FAILURE);
+	}
+
+	// send controller data
+	for(unsigned int i=0; i<data_vec.size(); i++)
+	{
+		config_str << data_vec[i] << std::endl;
+	}
+
+	// close file
+	config_str.close();
+}
+
+/*! \brief check if a file exists
+ *
+ * \param[in] name name (with path) of the file to check
+ * \return true if file exists, 0 otherwise
+ */
+bool MultiSimu::CheckFileExist(const std::string &name)
+{
+	if (FILE *file = fopen(name.c_str(), "r"))
+	{
+		fclose(file);
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
diff --git a/userFiles/simu/parallel/MultiSimu.hh b/userFiles/simu/parallel/MultiSimu.hh
new file mode 100644
index 0000000..2de79a3
--- /dev/null
+++ b/userFiles/simu/parallel/MultiSimu.hh
@@ -0,0 +1,32 @@
+/*! 
+ * \author Nicolas Van der Noot
+ * \file MultiSimu.hh
+ * \brief MultiSimu class
+ */
+
+#include "ComputationSimu.hh"
+#include <string.h>
+#include <vector>
+
+/*! \brief multiple parallel simulation gestion
+ */
+class MultiSimu: public ComputationSimu
+{
+	public:
+		MultiSimu(MbsData *mbs_data, ModelSimuIndex *simu_index);
+		virtual ~MultiSimu();
+
+		virtual void Compute();
+
+	private:
+		std::string config_txt; ///< configuration file
+		std::string start_txt;  ///< start file
+
+		std::vector<double> data_vec; ///< controller data vec
+
+		// function prototypes
+		void WriteStart();
+		void WriteState();
+		bool CheckFileExist(const std::string &name);
+};
+
diff --git a/userFiles/simu/useful/low_filter/LowFilterSimu.cc b/userFiles/simu/useful/low_filter/LowFilterSimu.cc
index 5dd780f..0a50575 100644
--- a/userFiles/simu/useful/low_filter/LowFilterSimu.cc
+++ b/userFiles/simu/useful/low_filter/LowFilterSimu.cc
@@ -55,3 +55,16 @@ void LowFilterSimu::UpdateParams(double new_t)
 		f2 = 1.0;
 	}
 }
+
+/*! \brief reset the low-pass filter
+ *
+ * \param[in] current time [s]
+ */
+void LowFilterSimu::Reset(double cur_t)
+{
+	last_t = cur_t;
+	delta_t = 0.0;
+
+	f1 = 0.0;
+	f2 = 1.0;
+}
diff --git a/userFiles/simu/useful/low_filter/LowFilterSimu.hh b/userFiles/simu/useful/low_filter/LowFilterSimu.hh
index c777237..f029510 100644
--- a/userFiles/simu/useful/low_filter/LowFilterSimu.hh
+++ b/userFiles/simu/useful/low_filter/LowFilterSimu.hh
@@ -45,6 +45,8 @@ class LowFilterSimu
 			value = LowPass(value, new_val);
 		}
 
+		void Reset(double cur_t);
+
 	private:
 		double tau;     ///< time constant [s]
 		double last_t;  ///< last time value added [s]
diff --git a/userFiles/simu/useful/noise/NoiseSimu.cc b/userFiles/simu/useful/noise/NoiseSimu.cc
index 86db7d6..5f351e6 100644
--- a/userFiles/simu/useful/noise/NoiseSimu.cc
+++ b/userFiles/simu/useful/noise/NoiseSimu.cc
@@ -49,3 +49,17 @@ void NoiseSimu::Update(double t)
     	cur_n = (next_n - last_n) * (diff_t / t_period) + last_n;
     }
 }
+
+/*! \brief reset noise
+ * 
+ * \param[in] t current time [s]
+ */
+void NoiseSimu::Reset(double t)
+{
+    last_t = t;
+
+    last_n = NewNoise();
+    next_n = NewNoise();
+    cur_n  = last_n;
+}
+
diff --git a/userFiles/simu/useful/noise/NoiseSimu.hh b/userFiles/simu/useful/noise/NoiseSimu.hh
index 4ec3119..5a26073 100644
--- a/userFiles/simu/useful/noise/NoiseSimu.hh
+++ b/userFiles/simu/useful/noise/NoiseSimu.hh
@@ -19,6 +19,8 @@ class NoiseSimu
 
 		void Update(double t);
 
+		void Reset(double t);
+
 		/*! \brief get current noise level
 		 * 
 		 * \return current noise value, bounded in [-max_noise ; max_noise]
diff --git a/userfctR/CMakeLists.txt b/userfctR/CMakeLists.txt
index 2da190c..206a6b3 100644
--- a/userfctR/CMakeLists.txt
+++ b/userfctR/CMakeLists.txt
@@ -1,117 +1,146 @@
-# Copyright: (C)
-# Authors: Timothee Habra & Nicolas Van der Noot
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                       LIBRARY MAIN CONFIGURATIONS
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-cmake_minimum_required(VERSION 2.8.7)
-
-project(Project_userfct)
-
-# flags
-if (UNIX)
-    set(CMAKE_C_FLAGS "-fPIC")
-    set(CMAKE_EXE_LINKER_FLAGS "-fPIC")
-endif (UNIX)
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                        SEPARATE COMPILATION
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-if ( FLAG_SEPARATE_USER_FCT )
-
-    # additional CMakeLists.txt
-    set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
-    set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
-
-    add_subdirectory ( ${CMAKE_AUX}/flags/   ${CMAKE_AUX_BIN}/flags/   )
-    add_subdirectory ( ${CMAKE_AUX}/listing/ ${CMAKE_AUX_BIN}/listing/ )
-    add_subdirectory ( ${CMAKE_AUX}/libraries/ ${CMAKE_AUX_BIN}/libraries/ )
-
-    # real-time option
-    option (FLAG_REAL_TIME "Real time" OFF)
-
-    if(FLAG_REAL_TIME)
-        add_definitions( -DREAL_TIME )
-
-        # configure a header file to pass some of the CMake settings to the source code
-        configure_file (
-            "${ROBOTRAN_SOURCE_DIR}/conf/cmake_config.h.in"
-            "${PROJECT_BINARY_DIR}/conf/cmake_config.h"
-        )
-        include_directories (${PROJECT_BINARY_DIR}/conf)
-
-        # plot-visu options
-        option (FLAG_PLOT "Real time" OFF)
-        option (FLAG_VISU "Real time" OFF)
-
-        if(FLAG_PLOT)
-            add_definitions( -DSDL )
-        endif( )
-
-        if(FLAG_VISU)
-            add_definitions( -DJAVA )
-        endif( )
-
-    endif ( )
-endif ( )
-
-# release of debug
-release_debug()
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         SET LIBRARY
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-# list source files to compile
-init_src()
-increment_src ( ${PROJECT_SOURCE_DIR} )
-increment_src ( ${PROJECT_SOURCE_DIR}/../userFiles )
-increment_src ( ${PROJECT_SOURCE_DIR}/../dataR/index/${COMAN_VERSION} )
-
-# list include directories (to find headers)
-init_include()
-increment_include( ${ROBOTRAN_SOURCE_DIR}/mbs_common )
-increment_include( ${PROJECT_SOURCE_DIR}/../userFiles )
-increment_include( ${PROJECT_SOURCE_DIR}/../dataR/index/${COMAN_VERSION} )
-
-# include these directories
-include_directories ( ${INCLUDE_DIR} )
-include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
-
-if( FLAG_SHARED_LIB OR FLAG_SEPARATE_USER_FCT )
-    add_library(Project_userfct SHARED ${SOURCE_FILES})
-
-    target_link_libraries (Project_userfct ${LIB_MBSYSC_REALTIME})
-else()
-    add_library(Project_userfct STATIC ${SOURCE_FILES})
-    add_definitions( -DMBSYSC_UTILITIES_STATIC_DEFINE)
-	add_definitions( -DPROJECT_SYMBOLIC_STATIC_DEFINE)
-	add_definitions( -DMBSYSC_REALTIME_STATIC_DEFINE)
-
-    target_link_libraries (Project_userfct ${LIB_MBSYSC_REALTIME} ${LIB_MBSYSC_UTILITIES})
-endif()
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         LINK LIBRARIES
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-# MBSysC libraries
-if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
-    find_package( LibRobotranC REQUIRED )
-    add_definitions(${LIB_MBSYSC_DEFINITIONS})
-endif()
-
-if (FLAG_PLOT)
-    sdl_header_lib(userfct)
-endif ( )
-
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         STUFF FOR WINDOWS (TO BE CLEANED)
-# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-
-include (GenerateExportHeader)
-GENERATE_EXPORT_HEADER( Project_userfct )
-
-install (TARGETS Project_userfct DESTINATION ${CMAKE_INSTALL_PREFIX})
+#
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be  
+#   Contact : info@robotran.be
+#
+#
+# Authors: Timothee Habra, Nicolas Van der Noot, Nicolas Docquier, Olivier Lantsoght
+#
+# CMake for compiling the user files of a robotran project in C
+# 
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                       LIBRARY MAIN CONFIGURATIONS
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+if (UNIX)
+    cmake_minimum_required(VERSION 2.8.7)
+else()
+    cmake_minimum_required(VERSION 3.4)
+endif ()
+
+project(Project_userfct)
+
+# flags
+if (UNIX)
+    set(CMAKE_C_FLAGS "-fPIC")
+    set(CMAKE_EXE_LINKER_FLAGS "-fPIC")
+endif (UNIX)
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                        SEPARATE COMPILATION
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+if ( FLAG_SEPARATE_USER_FCT )
+
+    # additional CMakeLists.txt
+    set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
+    set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
+
+    add_subdirectory ( ${CMAKE_AUX}/flags/   ${CMAKE_AUX_BIN}/flags/   )
+    add_subdirectory ( ${CMAKE_AUX}/listing/ ${CMAKE_AUX_BIN}/listing/ )
+    add_subdirectory ( ${CMAKE_AUX}/libraries/ ${CMAKE_AUX_BIN}/libraries/ )
+
+    # real-time option
+    option (FLAG_REAL_TIME "Real time" OFF)
+
+    if(FLAG_REAL_TIME)
+        add_definitions( -DREAL_TIME )
+
+        # configure a header file to pass some of the CMake settings to the source code
+        configure_file (
+            "${ROBOTRAN_SOURCE_DIR}/conf/cmake_config.h.in"
+            "${PROJECT_BINARY_DIR}/conf/cmake_config.h"
+        )
+        include_directories (${PROJECT_BINARY_DIR}/conf)
+
+        # plot-visu options
+        option (FLAG_PLOT "Real time" OFF)
+        option (FLAG_VISU "Real time" OFF)
+        option (FLAG_JAVA "Real time" OFF)
+        option (FLAG_OPEN_GL "Real time" OFF)
+
+        if(FLAG_PLOT)
+            add_definitions( -DSDL )
+        endif( )
+
+        if(FLAG_VISU)
+            add_definitions( -DVISU_3D )
+        endif( )
+
+        if(FLAG_JAVA)
+            add_definitions( -DJAVA )
+        endif( )
+
+        if(FLAG_OPEN_GL)
+            add_definitions( -DOPEN_GL )
+        endif( )
+
+    endif ( )
+endif ( )
+
+# release of debug
+release_debug()
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         SET LIBRARY
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# list source files to compile
+init_src()
+increment_src( ${PROJECT_SOURCE_DIR} )
+increment_void_user( ${PROJECT_SOURCE_DIR} )
+increment_src( ${PROJECT_SOURCE_DIR}/../userFiles )
+increment_src( ${PROJECT_SOURCE_DIR}/../dataR/extra/${COMAN_VERSION} )
+
+# list include directories (to find headers)
+init_include()
+increment_include( ${ROBOTRAN_SOURCE_DIR}/mbs_common )
+increment_include( ${PROJECT_SOURCE_DIR}/../userFiles )
+increment_include( ${PROJECT_SOURCE_DIR}/../dataR/extra/${COMAN_VERSION} )
+
+# include these directories
+include_directories ( ${INCLUDE_DIR} )
+include_directories ( ${CMAKE_CURRENT_BINARY_DIR} )
+
+if( FLAG_SHARED_LIB OR FLAG_SEPARATE_USER_FCT )
+    set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+    add_library(Project_userfct SHARED ${SOURCE_FILES})
+
+    target_link_libraries (Project_userfct ${LIB_MBSYSC_REALTIME})
+    target_link_libraries (Project_userfct ${LIB_MBSYSC_MODULES})
+    target_link_libraries (Project_userfct ${LIB_MBSYSC_UTILITIES})
+    
+    # MacOS: Force extension to be .so rather than .dylib
+    if(APPLE)
+        set_target_properties(Project_userfct PROPERTIES SUFFIX ".so")    
+    endif()
+
+    # Windows, copy dll next to executable
+    if((NOT UNIX) AND (NOT FLAG_SEPARATE_USER_FCT))
+        add_custom_command(TARGET Project_userfct POST_BUILD
+                COMMAND ${CMAKE_COMMAND} -E copy
+                ${CMAKE_CURRENT_BINARY_DIR}/Debug/Project_userfct.dll
+                ${CMAKE_CURRENT_BINARY_DIR}/../Debug/Project_userfct.dll
+        )
+    endif()
+
+else()
+    add_library(Project_userfct STATIC ${SOURCE_FILES})
+    target_link_libraries (Project_userfct ${LIB_MBSYSC_REALTIME} ${LIB_MBSYSC_UTILITIES})
+endif()
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         LINK LIBRARIES
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
+# MBSysC libraries
+if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
+    find_package( LibRobotranC REQUIRED )
+    add_definitions(${LIB_MBSYSC_DEFINITIONS})
+endif()
+
+if (FLAG_PLOT)
+    sdl_header_lib(userfct)
+endif ( )
diff --git a/userfctR/realtime/user_realtime_events.cc b/userfctR/realtime/user_realtime_events.cc
index cdefa0d..05db37c 100644
--- a/userfctR/realtime/user_realtime_events.cc
+++ b/userfctR/realtime/user_realtime_events.cc
@@ -158,8 +158,12 @@ void user_keyboard(MbsData* mbs_data, Simu_realtime *realtime, int cur_t_usec, c
 
 
 	// uCtrl->V_fwd() = MEAN_VELOCITY_REQUEST + SEMI_DIFF_VELOCITY_REQUEST * (uCtrl->Arrows(0).Get()/100.0);
-	uCtrl->S_length() = MEAN_STEPLENGTH_REQUEST + SEMI_DIFF_STEPLENGTH_REQUEST * (uCtrl->Arrows(1).Get()/100.0);
-	uCtrl->S_height() = MEAN_STEPHEIGHT_REQUEST + SEMI_DIFF_STEPHEIGHT_REQUEST * (uCtrl->Arrows(0).Get()/100.0);
+	//uCtrl->S_length() = MEAN_STEPLENGTH_REQUEST + SEMI_DIFF_STEPLENGTH_REQUEST * (uCtrl->Arrows(1).Get()/100.0);
+	//uCtrl->S_height() = MEAN_STEPHEIGHT_REQUEST + SEMI_DIFF_STEPHEIGHT_REQUEST * (uCtrl->Arrows(0).Get()/100.0);
+
+	// TEMPORARY
+	uCtrl->S_length() = 0.4;
+	uCtrl->S_height() = 0.06;
 }
 
 /*! \brief handle inputs comming from joysticks axes
diff --git a/userfctR/realtime/user_realtime_options.cc b/userfctR/realtime/user_realtime_options.cc
index 92991b4..1005112 100644
--- a/userfctR/realtime/user_realtime_options.cc
+++ b/userfctR/realtime/user_realtime_options.cc
@@ -103,7 +103,7 @@ void user_realtime_options(MbsData* mbs_data, Realtime_option *options)
 	options->flag_visu = 45.0;
 
 	// initial viewpoint
-	options->start_viewpoint = 1;
+	options->start_viewpoint = 0;
 
 	// finish with break
 	options->final_break = 1;
diff --git a/workR/CMakeLists.txt b/workR/CMakeLists.txt
index 96ae22e..ce429ef 100644
--- a/workR/CMakeLists.txt
+++ b/workR/CMakeLists.txt
@@ -1,7 +1,12 @@
 #
-# Authors: Nicolas Van der Noot, Nicolas Docquier, Timothee Habra
+#   Universite catholique de Louvain
+#   CEREM : Centre for research in mechatronics
+#   http://www.robotran.be
+#   Contact : info@robotran.be
 #
-# CMake for the COMAN project 
+# Authors: Nicolas Docquier, Nicolas Van der Noot, Timothee Habra, Olivier Lantsoght
+#
+# CMake for the COMAN project
 #
 
 
@@ -39,7 +44,11 @@ set ( SALMAN_CTRL OFF )
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
 # CMake minimum version
-cmake_minimum_required(VERSION 2.8.7)
+if (UNIX)
+	cmake_minimum_required(VERSION 2.8.7)
+else()
+	cmake_minimum_required(VERSION 3.4)
+endif ()
 
 # project name
 project (projectRobotran)
@@ -48,17 +57,19 @@ project (projectRobotran)
 set( CMAKE_C_FLAGS_RELEASE   "-O3" )
 set( CMAKE_CXX_FLAGS_RELEASE "-O3" )
 
-# variable for storing the path to Robotran common files
-set(TRIAL_PATHS_MBSYSC
-	"D:/Documents/Github/mbsysc/MBsysC"				   # phil asus
-	"/home/pgreiner/Documents/mbsysc/MBsysC"			   # phil biorob linux
-	"/home/nico/Documents/Robotran/Projects/robotran/mbsysc/MBsysC"    # Nico ThinkPad Linux
-	"/data/nvander/Documents/Robotran/Projects/robotran/mbsysc/MBsysC" # Nico BioRob Linux
-	"C:/Users/Nico/Documents/Robotran/Projects/mbsysc/MBsysC"          # Nico ThinkPad Windows
-	"/Users/MarieSophie/Documents/Robotran/Projects/mbsysc/MBsysC"     # Mary Mac OS
-	"${PROJECT_SOURCE_DIR}/../mbsysCopy"                               # copy folder
-)
-find_path(ROBOTRAN_SOURCE_DIR mbs_common ${TRIAL_PATHS_MBSYSC} DOC "Path to the Robotran-MBsysC common files")
+# Variable for storing the path to Robotran common files (should be adapted depending on the location of those source)
+if(NOT DEFINED ROBOTRAN_SOURCE_DIR)
+	set(TRIAL_PATHS_MBSYSC
+		/home/nico/Documents/Robotran/Projects/robotran/mbsysc/MBsysC    # Nico ThinkPad Linux
+		/data/nvander/Documents/Robotran/Projects/robotran/mbsysc/MBsysC # Nico BioRob Linux
+		C:/Users/Nico/Documents/Robotran/Projects/mbsysc/MBsysC          # Nico ThinkPad Windows
+		/Users/MarieSophie/Documents/Robotran/Projects/mbsysc/MBsysC     # Mary Mac OS
+		D:/Documents/Github/mbsysc/MBsysC      # phil asus
+		/home/pgreiner/Documents/mbsysc/MBsysC # phil biorob linux
+		${PROJECT_SOURCE_DIR}/../mbsysCopy # copy folder
+	)
+	find_path(ROBOTRAN_SOURCE_DIR mbs_common ${TRIAL_PATHS_MBSYSC} DOC "Path to the Robotran-MBsysC common files")
+endif ( )
 
 # variable for storing the path to Salman controller project (if any)
 set (TRIAL_PATHS_SALMAN_WBO
@@ -75,70 +86,68 @@ endif ( )
 
 # message to display the project name and COMAN version
 message(STATUS "Processing ${PROJECT_NAME}")
+message(STATUS "MBsysC path: ${ROBOTRAN_SOURCE_DIR}")
 message(STATUS "CoMan version: ${COMAN_VERSION}")
-message(STATUS "MBsysC dir: ${ROBOTRAN_SOURCE_DIR}")
 message(STATUS "Speed ref: ${CMAKE_V_REF}")
 message(STATUS "Height ref: ${CMAKE_H_REF}")
 message(STATUS "Length ref: ${CMAKE_L_REF}")
 
-# for Unix: display all the warnings, except the ones related to /* -- */ and for the unused variables
+# for Unix: display all the warnings, except the ones related to /* -- */, to unused variables and to unknown warnings
 if (UNIX)
-	set( CMAKE_C_FLAGS "-g -Wall -Wno-unused-but-set-variable -Wno-unused-variable -Wno-comment" )
+	set( CMAKE_C_FLAGS "-g -Wall -Wno-unused-but-set-variable -Wno-unused-variable -Wno-comment -Wno-unknown-warning-option" )
 endif ( )
 
 
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
-#                         Additional CMakelists.txt
+#                         Windows libraries
 # * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 
-## ---- WINDOWS DLL FILES ---- ##
-
-# copy all the dll (except 'jvm.dll') used for Windows
-# these dll files are copied in the Executable directory (Debug or Release)
 if (WIN32)
 
-    # avoid warnings
-    if(COMMAND cmake_policy)
-        cmake_policy(SET CMP0054 NEW)
-    endif()
-
-    # Visual studio version
-    if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
-        if (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 18.0)
-            message("Visual Studio version set to VS2012 (${CMAKE_CXX_COMPILER_VERSION})")
-            set(MSVC_VERSION lib-vc2012)
-        elseif (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 19.0)
-            message("Visual Studio version set to VS2013 (${CMAKE_CXX_COMPILER_VERSION})")
-            set(MSVC_VERSION lib-vc2013)
-        else ( )
-            message("Visual Studio version set to VS2015 (${CMAKE_CXX_COMPILER_VERSION})")
-            set(MSVC_VERSION lib-vc2015)
-        endif ( )
-    else ( )
-        message(FATAL_ERROR "CMake error: only Visual Studio projects can currently be used on Windows !")
-    endif ( )
-
-    ## --- WIN32 or WIN64 DETECTION --- ##
-    if( CMAKE_SIZEOF_VOID_P EQUAL 8 )   # 64bit Windows
-        SET(WIN_LIB_DIRECTORY win64_include_lib)
-    else( )  # 32bit Windows
-        SET(WIN_LIB_DIRECTORY win32_include_lib)
-    endif( )
-
-    ## ---- WINDOWS DLL FILES ---- ##
-
-    # copy all the dll (except 'jvm.dll') used for Windows
-    # these dll files are copied in the Executable directory (Debug or Release)
-    file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
-    file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+	# avoid warnings
+	if(COMMAND cmake_policy)
+		cmake_policy(SET CMP0054 NEW)
+	endif()
+
+	# Visual studio version
+	if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
+		if (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 18.0)
+			message("Visual Studio version set to VS2012 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2012)
+		elseif (${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 19.0)
+			message("Visual Studio version set to VS2013 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2013)
+		else ( )
+			message("Visual Studio version set to VS2015 (${CMAKE_CXX_COMPILER_VERSION})")
+			set(MSVC_VERSION lib-vc2015)
+		endif ( )
+	else ( )
+		message(FATAL_ERROR "CMake error: only Visual Studio projects can currently be used on Windows !")
+	endif ( )
+
+	## --- WIN32 or WIN64 DETECTION --- ##
+	if( CMAKE_SIZEOF_VOID_P EQUAL 8 )   # 64bit Windows
+		SET(WIN_LIB_DIRECTORY win64_include_lib)
+	else( )  # 32bit Windows
+		SET(WIN_LIB_DIRECTORY win32_include_lib)
+	endif( )
+
+	## ---- WINDOWS DLL FILES ---- ##
+
+	# copy all the dll (except 'jvm.dll') used for Windows
+	# these dll files are copied in the Executable directory (Debug or Release)
 	
+	# common dll
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+
 	# GLPK dll
 	file(COPY ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_win/glpk/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
 	file(COPY ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_win/glpk/${WIN_LIB_DIRECTORY}/dll/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
 
-    # GLFW
-    file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
-    file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
+	# GLFW dll
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Debug)
+	file(COPY ${ROBOTRAN_SOURCE_DIR}/${WIN_LIB_DIRECTORY}/dll/GLFW/${MSVC_VERSION}/ DESTINATION ${CMAKE_BINARY_DIR}/Release)
 endif ( )
 
 # install prefix
@@ -147,6 +156,11 @@ set(CMAKE_INSTALL_PREFIX ${CMAKE_BINARY_DIR}/Debug)
 # find libraries paths
 set(CMAKE_MODULE_PATH ${ROBOTRAN_SOURCE_DIR}/conf ${PROJECT_SOURCE_DIR}/../mbsysCopy/extra_conf)
 
+
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+#                         Additional CMakelists.txt
+# * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+
 # additional CMake functions
 set(CMAKE_AUX ${ROBOTRAN_SOURCE_DIR}/cmake_aux)
 set(CMAKE_AUX_BIN ${PROJECT_BINARY_DIR}/cmake_aux)
@@ -461,24 +475,31 @@ if ( FLAG_SEPARATE_BUILD ) # find MBSysC dynamic libraries
 
 		## ---- WINDOWS DLL FILES ---- ##
 
+		# find dll directories (Debug or Release)
+		get_filename_component(LIB_MBSYSC_MODULES_DIR ${LIB_MBSYSC_MODULES} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_LOAD_DIR ${LIB_MBSYSC_LOAD} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_UTILITIES_DIR ${LIB_MBSYSC_UTILITIES} DIRECTORY)
+		get_filename_component(LIB_MBSYSC_REALTIME_DIR ${LIB_MBSYSC_REALTIME} DIRECTORY)
+
 		# copy all the dll used for Windows
 		# these dll files are copied in the Executable directory (Debug)
-		file(COPY ${ROBOTRAN_SOURCE_DIR}\\build\\mbs_module\\Debug\\MBsysC_module.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
-		file(COPY ${ROBOTRAN_SOURCE_DIR}\\build\\mbs_load_xml\\Debug\\MBsysC_loadXML.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
-		file(COPY ${ROBOTRAN_SOURCE_DIR}\\build\\mbs_utilities\\Debug\\MBsysC_utilities.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
-		file(COPY ${ROBOTRAN_SOURCE_DIR}\\build\\mbs_realtime\\Debug\\MBsysC_realtime.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_MODULES_DIR}\\MBsysC_module.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_LOAD_DIR}\\MBsysC_loadXML.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_UTILITIES_DIR}\\MBsysC_utilities.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+		file(COPY ${LIB_MBSYSC_REALTIME_DIR}\\MBsysC_realtime.dll DESTINATION ${CMAKE_BINARY_DIR}\\Debug)
+
 	endif ( )
 
-else ( ) 
+else ( )
 
 	target_link_libraries( ${Executable} MBsysC_loadXML MBsysC_module )
 
 	if (NOT FLAG_SHARED_LIB)
 
 		# find MBSysC static libraries
-		target_link_libraries( ${Executable} MBsysC_struct MBsysC_numerics MBsysC_realtime MBsysC_utilities )
+		target_link_libraries( ${Executable} MBsysC_numerics MBsysC_realtime MBsysC_utilities )
 
-		# user function and symbolic libraries
+		#user functions library
 		target_link_libraries ( ${Executable} Project_userfct )
 		target_link_libraries ( ${Executable} Project_symbolic )
 
@@ -491,12 +512,15 @@ else ( )
 		endif ( )
 
 		# Java external library
-		if (FLAG_VISU)
+		if (FLAG_JAVA)
 			target_link_libraries ( ${Executable} ${JNI_LIBRARIES} )
 		endif ( )
 
-		add_definitions( -D MBSYSC_MODULE_STATIC_DEFINE )
+		# OpenGL external library
+		if (FLAG_OPEN_GL)
 
+		endif ( )
+			   
 	endif ( )
 
 endif ( )
@@ -506,14 +530,22 @@ include_directories(${LIB_MBSYSC_INCLUDE_DIRS})
 
 # symbolic files
 if ( FLAG_SEPARATE_SYMBOLIC )
-	file(COPY ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH}/build/libProject_symbolic.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
+	if( UNIX )
+		file(COPY ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH}/build/libProject_symbolic.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
+	else() # Windows (to be tested)
+		file(COPY ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH}/build/Debug/libProject_symbolic.dll DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Debug)
+	endif()
 else ( )
 	add_subdirectory( ${PROJECT_SOURCE_DIR}/../${SYMBOLIC_PATH} ${CMAKE_CURRENT_BINARY_DIR}/symbolicR)
 endif ( )
 
 # user fonction files
 if ( FLAG_SEPARATE_USER_FCT )
-	file(COPY ${PROJECT_SOURCE_DIR}/../userfctR/build/libProject_userfct.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
+	if( UNIX )
+		file(COPY ${PROJECT_SOURCE_DIR}/../userfctR/build/libProject_userfct.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
+	else() # Windows (to be tested)
+		file(COPY ${PROJECT_SOURCE_DIR}/../userfctR/build/Debug/libProject_userfct.dll DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/Debug)
+	endif()
 else ( )
 	add_subdirectory( ${PROJECT_SOURCE_DIR}/../userfctR ${CMAKE_CURRENT_BINARY_DIR}/userfctR)
 endif ( )
diff --git a/workR/src/simu_run.cc b/workR/src/simu_run.cc
index 76af1a2..7aafc67 100644
--- a/workR/src/simu_run.cc
+++ b/workR/src/simu_run.cc
@@ -111,7 +111,7 @@ double simu_run(RunInOut *run_in_out)
 	}
 
 	// results
-	mbs_dirdyn->options->save2file = 1;
+	mbs_dirdyn->options->save2file = 0;
 	mbs_dirdyn->options->saveperiod = 4;
 	mbs_dirdyn->options->max_save_user = 12;
 	mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
