From 8a4ef643fa7dcb211334af1aece53893b2bdddce Mon Sep 17 00:00:00 2001
From: Adrian Knobloch <adrian.knobloch@student.kit.edu>
Date: Mon, 26 Jun 2017 21:20:27 +0200
Subject: [PATCH] Add new functionality to SimDynamicsViewer

---
 .../SimDynamicsViewer/simDynamicsViewer.ui    | 1004 ++++++++++-------
 .../SimDynamicsViewer/simDynamicsWindow.cpp   |   38 +-
 .../SimDynamicsViewer/simDynamicsWindow.h     |    7 +
 3 files changed, 645 insertions(+), 404 deletions(-)

diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
index bac629bae..697a061f9 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsViewer.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>913</width>
-    <height>791</height>
+    <height>881</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -16,546 +16,744 @@
   <widget class="QWidget" name="centralwidget">
    <layout class="QGridLayout" name="gridLayout">
     <item row="0" column="1">
-     <widget class="QGroupBox" name="groupBox">
+     <widget class="QScrollArea" name="scrollArea">
+      <property name="sizePolicy">
+       <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+        <horstretch>0</horstretch>
+        <verstretch>0</verstretch>
+       </sizepolicy>
+      </property>
+      <property name="minimumSize">
+       <size>
+        <width>420</width>
+        <height>0</height>
+       </size>
+      </property>
       <property name="maximumSize">
        <size>
-        <width>400</width>
+        <width>420</width>
         <height>16777215</height>
        </size>
       </property>
-      <property name="title">
-       <string/>
+      <property name="widgetResizable">
+       <bool>true</bool>
       </property>
-      <layout class="QVBoxLayout" name="verticalLayout_2">
-       <property name="spacing">
-        <number>0</number>
+      <property name="alignment">
+       <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+      </property>
+      <widget class="QWidget" name="scrollAreaWidgetContents_3">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
        </property>
-       <item>
-        <layout class="QHBoxLayout" name="horizontalLayout">
-         <item>
-          <widget class="QPushButton" name="pushButtonLoad">
-           <property name="text">
-            <string>Load Robot</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <spacer name="horizontalSpacer_2">
-           <property name="orientation">
-            <enum>Qt::Horizontal</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>40</width>
-             <height>20</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pushButton_reloadRobot">
-           <property name="text">
-            <string>Reload Robot</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <spacer name="horizontalSpacer">
-           <property name="orientation">
-            <enum>Qt::Horizontal</enum>
-           </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>40</width>
-             <height>20</height>
-            </size>
-           </property>
-          </spacer>
-         </item>
-         <item>
-          <widget class="QPushButton" name="pushButtonAddObject">
-           <property name="text">
-            <string>Add Object</string>
-           </property>
-          </widget>
-         </item>
-        </layout>
-       </item>
-       <item>
-        <widget class="QCheckBox" name="checkBox_selfCol">
-         <property name="text">
-          <string>Enable self collision (requires robot reload)</string>
-         </property>
-         <property name="checked">
-          <bool>true</bool>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBox_5">
-         <property name="title">
-          <string>Joints</string>
-         </property>
-         <layout class="QGridLayout" name="gridLayout_4">
-          <item row="0" column="0">
-           <widget class="QCheckBox" name="checkBoxActuation">
-            <property name="text">
-             <string>Joint actuation</string>
-            </property>
-            <property name="checked">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="QLabel" name="label_7">
-            <property name="text">
-             <string>RobotNode</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_8">
+       <property name="minimumSize">
+        <size>
+         <width>400</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>400</width>
+         <height>16777215</height>
+        </size>
+       </property>
+       <layout class="QVBoxLayout" name="verticalLayout_2">
+        <property name="spacing">
+         <number>0</number>
+        </property>
+        <item>
+         <layout class="QHBoxLayout" name="horizontalLayout">
+          <item>
+           <widget class="QPushButton" name="pushButtonLoad">
             <property name="text">
-             <string>Target Joint Value</string>
+             <string>Load Robot</string>
             </property>
            </widget>
           </item>
-          <item row="1" column="1">
-           <widget class="QComboBox" name="comboBoxRobotNode"/>
-          </item>
-          <item row="2" column="0" colspan="2">
-           <widget class="QSlider" name="horizontalSliderTarget">
-            <property name="minimum">
-             <number>-100</number>
-            </property>
-            <property name="maximum">
-             <number>100</number>
-            </property>
+          <item>
+           <spacer name="horizontalSpacer_2">
             <property name="orientation">
              <enum>Qt::Horizontal</enum>
             </property>
-           </widget>
-          </item>
-          <item row="3" column="0">
-           <widget class="QLabel" name="label_TargetMin">
-            <property name="text">
-             <string>-1</string>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="1">
-           <widget class="QLabel" name="label_TargetMax">
-            <property name="layoutDirection">
-             <enum>Qt::RightToLeft</enum>
-            </property>
-            <property name="text">
-             <string>1</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBox_4">
-         <property name="title">
-          <string>Physics</string>
-         </property>
-         <layout class="QGridLayout" name="gridLayout_3">
-          <item row="0" column="2" colspan="2">
-           <widget class="QPushButton" name="pushButtonStep">
-            <property name="enabled">
-             <bool>false</bool>
-            </property>
-            <property name="text">
-             <string>Step Engine</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0" colspan="3">
-           <widget class="QLabel" name="label_10">
-            <property name="text">
-             <string>Fixed Time Step</string>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="4" colspan="2">
-           <widget class="QCheckBox" name="checkBoxFixedTimeStep">
-            <property name="text">
-             <string>Fixed Time Step</string>
-            </property>
-            <property name="checked">
-             <bool>false</bool>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
             </property>
-           </widget>
+           </spacer>
           </item>
-          <item row="1" column="3" colspan="3">
-           <widget class="QLabel" name="label_9">
+          <item>
+           <widget class="QPushButton" name="pushButton_reloadRobot">
             <property name="text">
-             <string>Physics Update timer</string>
+             <string>Reload Robot</string>
             </property>
            </widget>
           </item>
-          <item row="0" column="0" colspan="2">
-           <widget class="QPushButton" name="pushButtonStartStop">
-            <property name="text">
-             <string>Stop Engine</string>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="0" colspan="3">
-           <widget class="QSlider" name="horizontalSliderFixedTimeStep">
-            <property name="enabled">
-             <bool>false</bool>
-            </property>
-            <property name="minimum">
-             <number>1</number>
-            </property>
-            <property name="maximum">
-             <number>33</number>
-            </property>
-            <property name="singleStep">
-             <number>1</number>
-            </property>
-            <property name="pageStep">
-             <number>10</number>
-            </property>
+          <item>
+           <spacer name="horizontalSpacer">
             <property name="orientation">
              <enum>Qt::Horizontal</enum>
             </property>
-           </widget>
-          </item>
-          <item row="2" column="3" colspan="3">
-           <widget class="QSlider" name="horizontalSliderUpdateTimer">
-            <property name="minimum">
-             <number>5</number>
-            </property>
-            <property name="maximum">
-             <number>200</number>
-            </property>
-            <property name="singleStep">
-             <number>5</number>
-            </property>
-            <property name="pageStep">
-             <number>10</number>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
             </property>
-            <property name="orientation">
-             <enum>Qt::Horizontal</enum>
-            </property>
-           </widget>
+           </spacer>
           </item>
-          <item row="3" column="0">
-           <widget class="QLabel" name="label_TargetMin_3">
+          <item>
+           <widget class="QPushButton" name="pushButtonAddObject">
             <property name="text">
-             <string>1ms</string>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="1" colspan="2">
-           <widget class="QLabel" name="label_TargetMax_3">
-            <property name="layoutDirection">
-             <enum>Qt::RightToLeft</enum>
-            </property>
-            <property name="text">
-             <string>33ms</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="3" colspan="2">
-           <widget class="QLabel" name="label_TargetMin_2">
-            <property name="text">
-             <string>5ms</string>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="5">
-           <widget class="QLabel" name="label_TargetMax_2">
-            <property name="layoutDirection">
-             <enum>Qt::RightToLeft</enum>
-            </property>
-            <property name="text">
-             <string>200ms</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+             <string>Add Object</string>
             </property>
            </widget>
           </item>
          </layout>
-        </widget>
-       </item>
-       <item>
-        <spacer name="verticalSpacer">
-         <property name="orientation">
-          <enum>Qt::Vertical</enum>
-         </property>
-         <property name="sizeHint" stdset="0">
-          <size>
-           <width>20</width>
-           <height>40</height>
-          </size>
-         </property>
-        </spacer>
-       </item>
-       <item>
-        <widget class="QTabWidget" name="tabWidget">
-         <property name="currentIndex">
-          <number>1</number>
-         </property>
-         <widget class="QWidget" name="tab">
-          <attribute name="title">
-           <string>Joint Info</string>
-          </attribute>
-          <layout class="QVBoxLayout" name="verticalLayout_3">
-           <item>
-            <widget class="QLabel" name="label_RNName">
+        </item>
+        <item>
+         <widget class="QCheckBox" name="checkBox_selfCol">
+          <property name="text">
+           <string>Enable self collision (requires robot reload)</string>
+          </property>
+          <property name="checked">
+           <bool>true</bool>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QGroupBox" name="groupBox_5">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="title">
+           <string>Joints</string>
+          </property>
+          <layout class="QGridLayout" name="gridLayout_4">
+           <item row="4" column="0">
+            <widget class="QLabel" name="label_TargetMin">
              <property name="text">
-              <string>Name: &lt;not set&gt;</string>
+              <string>-1</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNValue">
+           <item row="4" column="1">
+            <widget class="QLabel" name="label_TargetMax">
+             <property name="layoutDirection">
+              <enum>Qt::RightToLeft</enum>
+             </property>
              <property name="text">
-              <string>Joint value: 0</string>
+              <string>1</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNTarget">
+           <item row="0" column="1">
+            <spacer name="horizontalSpacer_3">
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+             </property>
+             <property name="sizeHint" stdset="0">
+              <size>
+               <width>40</width>
+               <height>20</height>
+              </size>
+             </property>
+            </spacer>
+           </item>
+           <item row="0" column="0">
+            <widget class="QCheckBox" name="checkBoxActuation">
              <property name="text">
-              <string>Joint target: 0</string>
+              <string>Joint actuation</string>
+             </property>
+             <property name="checked">
+              <bool>true</bool>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNVelocity">
+           <item row="1" column="1">
+            <widget class="QComboBox" name="comboBoxRobotNode"/>
+           </item>
+           <item row="2" column="0">
+            <widget class="QLabel" name="label_8">
              <property name="text">
-              <string>Joint velocity: 0</string>
+              <string>Target Joint Value</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNVelocityTarget">
+           <item row="1" column="0">
+            <widget class="QLabel" name="label_7">
              <property name="text">
-              <string>Joint velocity target: 0</string>
+              <string>RobotNode</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNPosGP">
-             <property name="text">
-              <string>GlobalPosition (simox):</string>
+           <item row="3" column="0" colspan="2">
+            <widget class="QSlider" name="horizontalSliderTarget">
+             <property name="minimum">
+              <number>-100</number>
+             </property>
+             <property name="maximum">
+              <number>100</number>
+             </property>
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNRPYGP">
+          </layout>
+         </widget>
+        </item>
+        <item>
+         <widget class="QGroupBox" name="groupBox_2">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="title">
+           <string>Robot</string>
+          </property>
+          <layout class="QGridLayout" name="gridLayout_7">
+           <item row="0" column="0">
+            <widget class="QLabel" name="label_14">
              <property name="text">
-              <string>GlobalRPY (simox):</string>
+              <string>Position XYZ</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNPosVisu">
+           <item row="1" column="0">
+            <widget class="QLabel" name="label_15">
              <property name="text">
-              <string>VISU (simox):</string>
+              <string>Rotation RPY</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_ForceTorqueB">
-             <property name="text">
-              <string>ForceTorqueB: </string>
+           <item row="0" column="3">
+            <widget class="QDoubleSpinBox" name="spinBox_Pos_Z">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_RNPosCom">
-             <property name="text">
-              <string>Com</string>
+           <item row="0" column="1">
+            <widget class="QDoubleSpinBox" name="spinBox_Pos_X">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="1" column="3">
+            <widget class="QDoubleSpinBox" name="spinBox_Rot_Y">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_ForceTorqueA">
+           <item row="1" column="1">
+            <widget class="QDoubleSpinBox" name="spinBox_Rot_R">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="1" column="2">
+            <widget class="QDoubleSpinBox" name="spinBox_Rot_P">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="2">
+            <widget class="QDoubleSpinBox" name="spinBox_Pos_Y">
+             <property name="minimum">
+              <double>-9999999999.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>9999999999.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="2" column="3">
+            <widget class="QPushButton" name="button_set">
              <property name="text">
-              <string>ForceTorqueA: </string>
+              <string>Set</string>
              </property>
             </widget>
            </item>
-           <item>
-            <widget class="QLabel" name="label_ForceTorqueJoint">
+           <item row="2" column="0">
+            <widget class="QPushButton" name="button_reset">
              <property name="text">
-              <string>ForceTorqueJoint: </string>
+              <string>Reset</string>
              </property>
             </widget>
            </item>
           </layout>
          </widget>
-         <widget class="QWidget" name="tab_2">
-          <attribute name="title">
-           <string>Robot Info</string>
-          </attribute>
-          <layout class="QFormLayout" name="formLayout">
-           <item row="0" column="0">
-            <widget class="QLabel" name="label_2">
+        </item>
+        <item>
+         <widget class="QGroupBox" name="groupBox_4">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="title">
+           <string>Physics</string>
+          </property>
+          <layout class="QGridLayout" name="gridLayout_3">
+           <item row="2" column="3" colspan="3">
+            <widget class="QLabel" name="label_9">
              <property name="text">
-              <string>Global Position</string>
+              <string>Physics Update timer</string>
              </property>
             </widget>
            </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_RobotPos">
+           <item row="0" column="0" colspan="2">
+            <widget class="QPushButton" name="pushButtonStartStop">
              <property name="text">
-              <string>0 / 0 / 0</string>
+              <string>Stop Engine</string>
              </property>
             </widget>
            </item>
-           <item row="1" column="0">
-            <widget class="QLabel" name="label_3">
+           <item row="2" column="0" colspan="3">
+            <widget class="QLabel" name="label_10">
              <property name="text">
-              <string>Global RPY</string>
+              <string>Fixed Time Step</string>
              </property>
             </widget>
            </item>
-           <item row="1" column="1">
-            <widget class="QLabel" name="label_RobotRPY">
+           <item row="0" column="4" colspan="2">
+            <widget class="QCheckBox" name="checkBoxFixedTimeStep">
              <property name="text">
-              <string>0 / 0 / 0</string>
+              <string>Fixed Time Step</string>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
              </property>
             </widget>
            </item>
-           <item row="2" column="0">
-            <widget class="QLabel" name="label_4">
+           <item row="4" column="5">
+            <widget class="QLabel" name="label_TargetMax_2">
+             <property name="layoutDirection">
+              <enum>Qt::RightToLeft</enum>
+             </property>
              <property name="text">
-              <string>COM</string>
+              <string>200ms</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
              </property>
             </widget>
            </item>
-           <item row="2" column="1">
-            <widget class="QLabel" name="label_RobotCom">
+           <item row="4" column="1" colspan="2">
+            <widget class="QLabel" name="label_TargetMax_3">
+             <property name="layoutDirection">
+              <enum>Qt::RightToLeft</enum>
+             </property>
              <property name="text">
-              <string>0 / 0 / 0</string>
+              <string>33ms</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
              </property>
             </widget>
            </item>
-           <item row="3" column="0">
-            <widget class="QLabel" name="label_5">
+           <item row="4" column="3" colspan="2">
+            <widget class="QLabel" name="label_TargetMin_2">
              <property name="text">
-              <string>RootNode Pos</string>
+              <string>5ms</string>
              </property>
             </widget>
            </item>
-           <item row="4" column="0">
-            <widget class="QLabel" name="label_6">
+           <item row="0" column="2" colspan="2">
+            <widget class="QPushButton" name="pushButtonStep">
+             <property name="enabled">
+              <bool>false</bool>
+             </property>
              <property name="text">
-              <string>RootNode RPY</string>
+              <string>Step Engine</string>
              </property>
             </widget>
            </item>
-           <item row="3" column="1">
-            <widget class="QLabel" name="label_RootNodePos">
-             <property name="text">
-              <string>0 / 0 / 0</string>
+           <item row="3" column="0" colspan="3">
+            <widget class="QSlider" name="horizontalSliderFixedTimeStep">
+             <property name="enabled">
+              <bool>false</bool>
+             </property>
+             <property name="minimum">
+              <number>1</number>
+             </property>
+             <property name="maximum">
+              <number>33</number>
+             </property>
+             <property name="singleStep">
+              <number>1</number>
+             </property>
+             <property name="pageStep">
+              <number>10</number>
+             </property>
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
              </property>
             </widget>
            </item>
-           <item row="4" column="1">
-            <widget class="QLabel" name="label_RootNodeRPY">
+           <item row="3" column="3" colspan="3">
+            <widget class="QSlider" name="horizontalSliderUpdateTimer">
+             <property name="minimum">
+              <number>5</number>
+             </property>
+             <property name="maximum">
+              <number>200</number>
+             </property>
+             <property name="singleStep">
+              <number>5</number>
+             </property>
+             <property name="pageStep">
+              <number>10</number>
+             </property>
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="0">
+            <widget class="QLabel" name="label_TargetMin_3">
              <property name="text">
-              <string>0 / 0 / 0</string>
+              <string>1ms</string>
              </property>
             </widget>
            </item>
-           <item row="5" column="0">
-            <widget class="QLabel" name="label_11">
+           <item row="1" column="0" colspan="6">
+            <layout class="QHBoxLayout" name="horizontalLayout_2">
+             <item>
+              <widget class="QLabel" name="label_13">
+               <property name="text">
+                <string>Simulation step count: </string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_simuStepCount">
+               <property name="text">
+                <string>0</string>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+          </layout>
+         </widget>
+        </item>
+        <item>
+         <widget class="QTabWidget" name="tabWidget">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="currentIndex">
+           <number>1</number>
+          </property>
+          <widget class="QWidget" name="tab">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <attribute name="title">
+            <string>Joint Info</string>
+           </attribute>
+           <layout class="QVBoxLayout" name="verticalLayout_3">
+            <item>
+             <widget class="QLabel" name="label_RNName">
+              <property name="text">
+               <string>Name: &lt;not set&gt;</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNValue">
+              <property name="text">
+               <string>Joint value: 0</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNTarget">
+              <property name="text">
+               <string>Joint target: 0</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNVelocity">
+              <property name="text">
+               <string>Joint velocity: 0</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNVelocityTarget">
+              <property name="text">
+               <string>Joint velocity target: 0</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNPosGP">
+              <property name="text">
+               <string>GlobalPosition (simox):</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNRPYGP">
+              <property name="text">
+               <string>GlobalRPY (simox):</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNPosVisu">
+              <property name="text">
+               <string>VISU (simox):</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_ForceTorqueB">
+              <property name="text">
+               <string>ForceTorqueB: </string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_RNPosCom">
+              <property name="text">
+               <string>Com</string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_ForceTorqueA">
+              <property name="text">
+               <string>ForceTorqueA: </string>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QLabel" name="label_ForceTorqueJoint">
+              <property name="text">
+               <string>ForceTorqueJoint: </string>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </widget>
+          <widget class="QWidget" name="tab_2">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
+           </property>
+           <attribute name="title">
+            <string>Robot Info</string>
+           </attribute>
+           <layout class="QFormLayout" name="formLayout">
+            <item row="0" column="0">
+             <widget class="QLabel" name="label_2">
+              <property name="text">
+               <string>Global Position</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <widget class="QLabel" name="label_RobotPos">
+              <property name="text">
+               <string>0 / 0 / 0</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QLabel" name="label_3">
+              <property name="text">
+               <string>Global RPY</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="1">
+             <widget class="QLabel" name="label_RobotRPY">
+              <property name="text">
+               <string>0 / 0 / 0</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QLabel" name="label_4">
+              <property name="text">
+               <string>COM</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_RobotCom">
+              <property name="text">
+               <string>0 / 0 / 0</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
+             <widget class="QLabel" name="label_5">
+              <property name="text">
+               <string>RootNode Pos</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="0">
+             <widget class="QLabel" name="label_6">
+              <property name="text">
+               <string>RootNode RPY</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="1">
+             <widget class="QLabel" name="label_RootNodePos">
+              <property name="text">
+               <string>0 / 0 / 0</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QLabel" name="label_RootNodeRPY">
+              <property name="text">
+               <string>0 / 0 / 0</string>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="0">
+             <widget class="QLabel" name="label_11">
+              <property name="text">
+               <string>Root LocTransf Pos</string>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="1">
+             <widget class="QLabel" name="label_RootLocalTransfPos">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="1">
+             <widget class="QLabel" name="label_RootLocalTransfRPY">
+              <property name="text">
+               <string/>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="0">
+             <widget class="QLabel" name="label_12">
+              <property name="text">
+               <string>Root LocTransf RPY</string>
+              </property>
+             </widget>
+            </item>
+           </layout>
+          </widget>
+         </widget>
+        </item>
+        <item>
+         <widget class="QGroupBox" name="groupBox_3">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="title">
+           <string>Visualization</string>
+          </property>
+          <layout class="QGridLayout" name="gridLayout_2">
+           <item row="0" column="0">
+            <widget class="QCheckBox" name="checkBoxColModel">
              <property name="text">
-              <string>Root LocTransf Pos</string>
+              <string>Collision Model</string>
              </property>
             </widget>
            </item>
-           <item row="5" column="1">
-            <widget class="QLabel" name="label_RootLocalTransfPos">
+           <item row="0" column="1">
+            <widget class="QLabel" name="label">
              <property name="text">
-              <string/>
+              <string>Anti Aliasing</string>
              </property>
             </widget>
            </item>
-           <item row="6" column="1">
-            <widget class="QLabel" name="label_RootLocalTransfRPY">
+           <item row="1" column="0">
+            <widget class="QCheckBox" name="checkBoxContacts">
              <property name="text">
-              <string/>
+              <string>Contacts</string>
              </property>
             </widget>
            </item>
-           <item row="6" column="0">
-            <widget class="QLabel" name="label_12">
+           <item row="1" column="1" rowspan="2">
+            <widget class="QSpinBox" name="spinBoxAntiAliasing">
+             <property name="maximum">
+              <number>6</number>
+             </property>
+            </widget>
+           </item>
+           <item row="2" column="0">
+            <widget class="QCheckBox" name="checkBoxCom">
              <property name="text">
-              <string>Root LocTransf RPY</string>
+              <string>CoM</string>
              </property>
             </widget>
            </item>
           </layout>
          </widget>
-        </widget>
-       </item>
-       <item>
-        <widget class="QGroupBox" name="groupBox_3">
-         <property name="title">
-          <string>Visualization</string>
-         </property>
-         <layout class="QGridLayout" name="gridLayout_2">
-          <item row="0" column="0">
-           <widget class="QCheckBox" name="checkBoxColModel">
-            <property name="text">
-             <string>Collision Model</string>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="QLabel" name="label">
-            <property name="text">
-             <string>Anti Aliasing</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QCheckBox" name="checkBoxContacts">
-            <property name="text">
-             <string>Contacts</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="1" rowspan="2">
-           <widget class="QSpinBox" name="spinBoxAntiAliasing">
-            <property name="maximum">
-             <number>6</number>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="0">
-           <widget class="QCheckBox" name="checkBoxCom">
-            <property name="text">
-             <string>CoM</string>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
-      </layout>
+        </item>
+       </layout>
+      </widget>
      </widget>
     </item>
     <item row="0" column="0">
      <widget class="QFrame" name="frameViewer">
+      <property name="sizePolicy">
+       <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+        <horstretch>0</horstretch>
+        <verstretch>0</verstretch>
+       </sizepolicy>
+      </property>
       <property name="frameShape">
        <enum>QFrame::StyledPanel</enum>
       </property>
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
index c52db5f78..a79e305f9 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp
@@ -104,9 +104,11 @@ SimDynamicsWindow::SimDynamicsWindow(std::string& sRobotFilename)
     // register callback
     float TIMER_MS = 30.0f;
     SoSensorManager* sensor_mgr = SoDB::getSensorManager();
-    timerSensor = new SoTimerSensor(timerCB, this);
+    timerSensor = new SoTimerSensor(timerCB, static_cast<void *>(this));
     timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f));
     sensor_mgr->insertTimerSensor(timerSensor);
+
+    viewer->addStepCallback(stepCB, static_cast<void *>(this));
 }
 
 
@@ -132,6 +134,15 @@ void SimDynamicsWindow::timerCB(void* data, SoSensor* /*sensor*/)
     window->updateContactVisu();
     window->updateComVisu();
 
+    window->UI.label_simuStepCount->setText(QString::number(window->simuStepCount));
+}
+
+void SimDynamicsWindow::stepCB(void *data, btScalar timeStep)
+{
+    SimDynamicsWindow* window = static_cast<SimDynamicsWindow*>(data);
+    VR_ASSERT(window);
+
+    window->simuStepCount++;
 }
 
 
@@ -159,6 +170,9 @@ void SimDynamicsWindow::setupUI()
     connect(UI.pushButtonAddObject, SIGNAL(clicked()), this, SLOT(addObject()));
     connect(UI.pushButton_reloadRobot, SIGNAL(clicked()), this, SLOT(reloadRobot()));
 
+    connect(UI.button_reset, SIGNAL(clicked()), this, SLOT(resetPose()));
+    connect(UI.button_set, SIGNAL(clicked()), this, SLOT(setPose()));
+
     /*connect(UI.pushButtonLoad, SIGNAL(clicked()), this, SLOT(selectRobot()));
     connect(UI.pushButtonClose, SIGNAL(clicked()), this, SLOT(closeHand()));
     connect(UI.pushButtonOpen, SIGNAL(clicked()), this, SLOT(openHand()));
@@ -1011,3 +1025,25 @@ void SimDynamicsWindow::reloadRobot()
         buildVisualization();
     }
 }
+
+void SimDynamicsWindow::resetPose()
+{
+    Eigen::Vector3f rpos = robot->getRootNode()->getGlobalPose().block<3,1>(0,3);
+    Eigen::Vector3f rrpy = VirtualRobot::MathTools::eigen4f2rpy(robot->getRootNode()->getGlobalPose());
+
+    UI.spinBox_Pos_X->setValue(rpos[0]);
+    UI.spinBox_Pos_Y->setValue(rpos[1]);
+    UI.spinBox_Pos_Z->setValue(rpos[2]);
+
+    UI.spinBox_Rot_R->setValue(rrpy[0]);
+    UI.spinBox_Rot_P->setValue(rrpy[1]);
+    UI.spinBox_Rot_Y->setValue(rrpy[2]);
+}
+
+void SimDynamicsWindow::setPose()
+{
+    Eigen::Matrix4f pose = VirtualRobot::MathTools::rpy2eigen4f(UI.spinBox_Rot_R->value(), UI.spinBox_Rot_P->value(), UI.spinBox_Rot_Y->value());
+    pose.block<3, 1>(0, 3) = Eigen::Vector3f(UI.spinBox_Pos_X->value(), UI.spinBox_Pos_Y->value(), UI.spinBox_Pos_Z->value());
+
+    dynamicsRobot->setGlobalPose(pose);
+}
diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
index adbe11a5a..92db29307 100644
--- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
+++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.h
@@ -28,6 +28,7 @@
 
 
 #include <vector>
+#include <atomic>
 
 #include "ui_simDynamicsViewer.h"
 
@@ -72,6 +73,9 @@ public slots:
 
     void reloadRobot();
 
+    void resetPose();
+    void setPose();
+
 protected:
     bool loadRobot(std::string robotFilename);
     void setupUI();
@@ -101,6 +105,7 @@ protected:
 
     // beside the viewer cb we need also a callback to update joint info
     static void timerCB(void* data, SoSensor* sensor);
+    static void stepCB(void* data, btScalar timeStep);
 
     SoTimerSensor* timerSensor;
 
@@ -110,6 +115,8 @@ protected:
 
     bool useColModel;
     std::string robotFilename;
+
+    std::atomic_uint_fast64_t simuStepCount{0};
 };
 
 #endif // __SimDynamics_WINDOW_H_
-- 
GitLab