diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index f4faf8ef47366c0837c1c78c285eeb8be29f14ca..34de6dfffd308bba91f6ad70ca2de2024020af20 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -42,6 +42,22 @@
propertyName="TCPControlUnitName"
propertyIsOptional="true"
propertyDefaultValue="TCPControlUnit" />
+ <Proxy include="RobotAPI/interface/units/HapticUnit.h"
+ humanName="Haptic Unit Observer"
+ typeName="HapticUnitObserverInterfacePrx"
+ memberName="hapticObserver"
+ getterName="getHapticObserver"
+ propertyName="HapticUnitObserverName"
+ propertyIsOptional="true"
+ propertyDefaultValue="HapticUnitObserver" />
+ <Proxy include="RobotAPI/interface/units/WeissHapticUnit.h"
+ humanName="Weiss Haptic Unit"
+ typeName="WeissHapticUnitInterfacePrx"
+ memberName="weissHapticUnit"
+ getterName="getWeissHapticUnit"
+ propertyName="WeissHapticUnitName"
+ propertyIsOptional="true"
+ propertyDefaultValue="WeissHapticUnit" />
<Proxy include="RobotAPI/interface/core/RobotState.h"
humanName="Robot State Component"
typeName="RobotStateComponentInterfacePrx"
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/CMakeLists.txt b/scenarios/tests/WeissHapticSensorsUnitTest/CMakeLists.txt
index 4d3f9b968efe4fc1217eef767d7ed8f436f223c4..b94e569387e346dc6b9001c8a0a9cb66af8de1d2 100644
--- a/scenarios/tests/WeissHapticSensorsUnitTest/CMakeLists.txt
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/CMakeLists.txt
@@ -1,15 +1,16 @@
+set(SCENARIO_CONFIG_COMPONENTS
+ config/RobotControl.cfg
+ config/WeissHapticUnitApp.cfg
+ config/XMLRemoteStateOfferer.WeissHapticGroup.cfg
+ config/SystemObserver.cfg
+ config/ConditionHandler.cfg
-set(SCENARIO_COMPONENTS
- WeissHapticSensorsUnitApp
- #WeissHapticSensorApp
- HapticObserverApp)
+ #WeissHapticUnitApp
+ #WeissHapticSensorApp
+ #HapticObserverApp
+)
-# optional 3rd parameter: "path/to/global/config.cfg"
-armarx_scenario("WeissHapticSensorsUnitTest" "${SCENARIO_COMPONENTS}")
-#set(SCENARIO_CONFIGS
-# config/ComponentName.optionalString.cfg
-# )
# optional 3rd parameter: "path/to/global/config.cfg"
-#armarx_scenario_from_configs("WeissHapticSensorsUnitTest" "${SCENARIO_CONFIGS}")
+armarx_scenario_from_configs("WeissHapticSensorsUnitTest" "${SCENARIO_CONFIG_COMPONENTS}")
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/config/ConditionHandler.cfg b/scenarios/tests/WeissHapticSensorsUnitTest/config/ConditionHandler.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..75f5b78f40cc1f83f2e59a4e1add7bf7d6235e98
--- /dev/null
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/config/ConditionHandler.cfg
@@ -0,0 +1,100 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Verbose
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Verbose
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.ConditionHandler properties
+# ==================================================================
+
+# ArmarX.ConditionHandler.HistoryLength: Length of condition history kept by the conditionhandler
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ConditionHandler.HistoryLength = 1000
+
+
+# ArmarX.ConditionHandler.Observers: Comma seperated observer list
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ConditionHandler.Observers = ""
+
+
+# ArmarX.ConditionHandler.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.ConditionHandler.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ConditionHandler.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ConditionHandler.ObjectName = ""
+
+
+
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/config/RobotControl.cfg b/scenarios/tests/WeissHapticSensorsUnitTest/config/RobotControl.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..08183f785f9ff82363676cbaa0206c66ad98ee6f
--- /dev/null
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/config/RobotControl.cfg
@@ -0,0 +1,117 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Verbose
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Verbose
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.DatapathConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/datapath.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DatapathConfig = ./config/datapath.cfg
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.RobotControlStateOfferer properties
+# ==================================================================
+
+# ArmarX.RobotControlStateOfferer.stateName: name of the state to load
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+ArmarX.RobotControlStateOfferer.stateName = "WeissHapticSensorTest"
+
+
+# ArmarX.RobotControlStateOfferer.proxyName: name of the proxy to load
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+ArmarX.RobotControlStateOfferer.proxyName = "WeissHapticGroupRemoteStateOfferer"
+
+
+# ArmarX.RobotControlStateOfferer.logstates:
+# Attributes:
+# - Default: Comma seperated list with state names to log. If not set, all transitions will be logged
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RobotControlStateOfferer.logstates = Comma seperated list with state names to log. If not set, all transitions will be logged
+
+
+# ArmarX.RobotControlStateOfferer.enableStatechartLogger: disable/enable statechart logger
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotControlStateOfferer.enableStatechartLogger = 0
+
+
+# ArmarX.RobotControlStateOfferer.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.RobotControlStateOfferer.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.RobotControlStateOfferer.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RobotControlStateOfferer.ObjectName = ""
+
+
+
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/config/SystemObserver.cfg b/scenarios/tests/WeissHapticSensorsUnitTest/config/SystemObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..f8cf074d1b8f678fc7d01f930487e2e71904d6f6
--- /dev/null
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/config/SystemObserver.cfg
@@ -0,0 +1,84 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Verbose
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Verbose
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.SystemObserver properties
+# ==================================================================
+
+# ArmarX.SystemObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.SystemObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.SystemObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.SystemObserver.ObjectName = ""
+
+
+
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/config/WeissHapticUnitApp.cfg b/scenarios/tests/WeissHapticSensorsUnitTest/config/WeissHapticUnitApp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..b8777df3dda7e5e6a369d2d71531ed9e217a0ea0
--- /dev/null
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/config/WeissHapticUnitApp.cfg
@@ -0,0 +1,121 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Verbose
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Verbose
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.HapticUnit properties
+# ==================================================================
+
+# ArmarX.HapticUnit.HapticTopicName: Name of the Haptic Topic.
+# Attributes:
+# - Default: HapticValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.HapticUnit.HapticTopicName = HapticValues
+
+
+# ArmarX.HapticUnit.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.HapticUnit.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HapticUnit.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+ArmarX.HapticUnit.ObjectName = "WeissHapticUnit"
+
+
+# ==================================================================
+# ArmarX.HapticUnitObserver properties
+# ==================================================================
+
+# ArmarX.HapticUnitObserver.HapticTopicName: Name of the HapticUnit Topic
+# Attributes:
+# - Default: HapticValues
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.HapticUnitObserver.HapticTopicName = HapticValues
+
+
+# ArmarX.HapticUnitObserver.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.HapticUnitObserver.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HapticUnitObserver.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.HapticUnitObserver.ObjectName = ""
+
+
+
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/config/XMLRemoteStateOfferer.WeissHapticGroup.cfg b/scenarios/tests/WeissHapticSensorsUnitTest/config/XMLRemoteStateOfferer.WeissHapticGroup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..caedd0eae1f142775184efe42a31195f80d7caa6
--- /dev/null
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/config/XMLRemoteStateOfferer.WeissHapticGroup.cfg
@@ -0,0 +1,91 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: ${HOME}/.armarx/mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Verbose
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Verbose
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = 0
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+ArmarX.ApplicationName = "XMLStateComponentWeissHapticGroup"
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.XMLStateComponent properties
+# ==================================================================
+
+# ArmarX.XMLStateComponent.XMLStatechartGroupDefinitionFile: Path to statechart group definition file (*.scgxml) - relative to projects source dir
+# Attributes:
+# - Case sensitivity: no
+# - Required: yes
+ArmarX.XMLStateComponent.XMLStatechartGroupDefinitionFile = RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
+
+
+# ArmarX.XMLStateComponent.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
+# ArmarX.XMLStateComponent.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.XMLStateComponent.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+ArmarX.XMLStateComponent.ObjectName = "XMLStateComponentWeissHapticGroup"
+
+
+
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/startGui.sh b/scenarios/tests/WeissHapticSensorsUnitTest/startGui.sh
deleted file mode 100755
index e2f4b07c6fc3db3a101c1ccfee1d6576e35b8b69..0000000000000000000000000000000000000000
--- a/scenarios/tests/WeissHapticSensorsUnitTest/startGui.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-export CORE_PATH=../../../Core
-export GUI_PATH=../../../Gui
-
-export SCRIPT_PATH=$CORE_PATH/build/bin
-export GUI_BIN_PATH=$GUI_PATH/build/bin
-
-# Gui
-$SCRIPT_PATH/startApplication.sh $GUI_BIN_PATH/ArmarXGuiRun &
diff --git a/scenarios/tests/WeissHapticSensorsUnitTest/tactile.html b/scenarios/tests/WeissHapticSensorsUnitTest/tactile.html
index ace11468986c6f332d36237c411da536fa9e78b7..5703a051b31ebdbd184d24262a1b1b25a0cc37bd 100755
--- a/scenarios/tests/WeissHapticSensorsUnitTest/tactile.html
+++ b/scenarios/tests/WeissHapticSensorsUnitTest/tactile.html
@@ -10,6 +10,12 @@ sensors = [];
</script>
<script src="ttyACM0.js"></script>
<script src="ttyACM1.js"></script>
+<script src="ttyACM2.js"></script>
+<script src="ttyACM3.js"></script>
+<script src="ttyACM4.js"></script>
+<script src="ttyACM5.js"></script>
+<script src="ttyACM6.js"></script>
+<script src="ttyACM7.js"></script>
<style type="text/css">
#out {
font-size: 10px;
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 6aa836f2e77b2de7ad076cce8d003f2a84cd2905..01bd7549a8ceae418036ba78a1dd44a89d8da769 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -3,8 +3,8 @@ add_subdirectory(ForceTorqueObserver)
add_subdirectory(HeadIKUnit)
add_subdirectory(TCPControlUnit)
-add_subdirectory(WeissHapticSensorsUnit)
-add_subdirectory(WeissHapticSensor)
+add_subdirectory(WeissHapticUnit)
+#add_subdirectory(WeissHapticSensor)
add_subdirectory(HapticObserver)
diff --git a/source/RobotAPI/applications/HapticObserver/CMakeLists.txt b/source/RobotAPI/applications/HapticObserver/CMakeLists.txt
index f1b7f7073f1873cb27033d76e8c8b99774d81b7b..f9205fbe4da5b3f0cfb440f7e9e68ca8f661c356 100644
--- a/source/RobotAPI/applications/HapticObserver/CMakeLists.txt
+++ b/source/RobotAPI/applications/HapticObserver/CMakeLists.txt
@@ -8,7 +8,7 @@ if (Eigen3_FOUND)
${Eigen3_INCLUDE_DIR})
endif()
-set(COMPONENT_LIBS WeissHapticSensorListener RobotAPIUnits)
+set(COMPONENT_LIBS RobotAPIUnits)
set(EXE_SOURCE main.cpp HapticObserverApp.h)
diff --git a/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt b/source/RobotAPI/applications/WeissHapticUnit/CMakeLists.txt
similarity index 71%
rename from source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt
rename to source/RobotAPI/applications/WeissHapticUnit/CMakeLists.txt
index ffc1889bb5753b0acf7cc6544ec0553928f12b97..3708373ea73b863f58052ffa6dc214e14e7f078b 100644
--- a/source/RobotAPI/applications/WeissHapticSensorsUnit/CMakeLists.txt
+++ b/source/RobotAPI/applications/WeissHapticUnit/CMakeLists.txt
@@ -1,4 +1,4 @@
-armarx_component_set_name("WeissHapticSensorsUnitApp")
+armarx_component_set_name("WeissHapticUnitApp")
find_package(Eigen3 QUIET)
armarx_build_if(Eigen3_FOUND "Eigen3 not available")
@@ -10,6 +10,6 @@ endif()
set(COMPONENT_LIBS RobotAPIUnits WeissHapticSensor )
-set(EXE_SOURCE main.cpp WeissHapticSensorsUnitApp.h)
+set(EXE_SOURCE main.cpp WeissHapticUnitApp.h)
armarx_add_component_executable("${EXE_SOURCE}")
diff --git a/source/RobotAPI/applications/WeissHapticSensorsUnit/WeissHapticSensorsUnitApp.h b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
similarity index 66%
rename from source/RobotAPI/applications/WeissHapticSensorsUnit/WeissHapticSensorsUnitApp.h
rename to source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
index 7c1d806ae0111ad4d4a32fcc3e42376d2e4d5058..4466a3b0e17882e97de29656ef6618e29e713425 100644
--- a/source/RobotAPI/applications/WeissHapticSensorsUnit/WeissHapticSensorsUnitApp.h
+++ b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
@@ -14,31 +14,29 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package RobotAPI::application::WeissHapticSensorsUnit
+ * @package RobotAPI::application::WeissHapticUnit
* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
* @date 2014
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
-#ifndef _ARMARX_APPLICATION_RobotAPI_WeissHapticSensorsUnit_H
-#define _ARMARX_APPLICATION_RobotAPI_WeissHapticSensorsUnit_H
-
-
-// #include <RobotAPI/components/@MyComponent@.h>
+#ifndef _ARMARX_APPLICATION_RobotAPI_WeissHapticUnit_H
+#define _ARMARX_APPLICATION_RobotAPI_WeissHapticUnit_H
#include <Core/core/application/Application.h>
-#include <RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.h>
+#include <RobotAPI/components/units/HapticObserver.h>
+#include <RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h>
namespace armarx
{
/**
- * @class WeissHapticSensorsUnitApp
- * @brief A brief description
+ * @class WeissHapticUnitApp
+ * @brief Joint Application for WeissHapticUnit and HapticObserver.
*
- * Detailed Description
+ * This Application runs both the WeissHapticUnit and the HapticObserver in one executable to avoid TCP communication.
*/
- class WeissHapticSensorsUnitApp :
+ class WeissHapticUnitApp :
virtual public armarx::Application
{
/**
@@ -47,7 +45,8 @@ namespace armarx
void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<WeissHapticSensorsUnit>(properties) );
+ registry->addObject( Component::create<WeissHapticUnit>(properties) );
+ registry->addObject( Component::create<HapticObserver>(properties) );
}
};
}
diff --git a/source/RobotAPI/applications/WeissHapticSensorsUnit/main.cpp b/source/RobotAPI/applications/WeissHapticUnit/main.cpp
similarity index 84%
rename from source/RobotAPI/applications/WeissHapticSensorsUnit/main.cpp
rename to source/RobotAPI/applications/WeissHapticUnit/main.cpp
index d2dffa82773301e9539add9c743338a94a70265d..488f764ec81279f8a74391c1f34f3ccf02ac0f7b 100644
--- a/source/RobotAPI/applications/WeissHapticSensorsUnit/main.cpp
+++ b/source/RobotAPI/applications/WeissHapticUnit/main.cpp
@@ -14,20 +14,20 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package RobotAPI::application::WeissHapticSensorsUnit
+ * @package RobotAPI::application::WeissHapticUnit
* @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
* @date 2014
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
-#include "WeissHapticSensorsUnitApp.h"
+#include "WeissHapticUnitApp.h"
#include <Core/core/logging/Logging.h>
int main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::WeissHapticSensorsUnitApp > ();
- app->setName("WeissHapticSensorsUnit");
+ armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::WeissHapticUnitApp > ();
+ app->setName("WeissHapticUnit");
return app->main(argc, argv);
}
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 278fe5e3f4df51d33277469499d09273f0a40eaa..8e093e38bd854f9eda7d03309749fd663000e23a 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -1,6 +1,6 @@
add_subdirectory(units)
-add_subdirectory(WeissHapticSensorListener)
+#add_subdirectory(WeissHapticSensorListener)
add_subdirectory(DebugDrawer)
add_subdirectory(MMMPlayer)
add_subdirectory(robotstate)
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 8f5d8293318f8518b978c448cd503816dd7be34f..9bb402e9665847f305f0bac5bd318e298bf84c7a 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -67,6 +67,7 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
+ ARMARX_INFO << "Offering new channel: " << channelName;
}
else
{
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index bec65d205749f93552d8e04712b07fbe33505444..4e167780a5f638c0302a97264df33501c536eadf 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -31,6 +31,7 @@
#include <Core/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <Core/core/services/tasks/PeriodicTask.h>
+
namespace armarx
{
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 2e6b02a6da616825e31b6ae27e9fd7277c92abc3..6b3b01d37b6cc5a9322da46287f87a559c217d67 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -250,6 +250,10 @@ namespace armarx
void TCPControlUnit::periodicExec()
{
+
+
+
+
{
ScopedTryLock lock(dataMutex);
@@ -274,10 +278,10 @@ namespace armarx
RemoteRobot::synchronizeLocalClone(localRobot,robotStateComponentPrx);
//ARMARX_DEBUG << "RN TCP R pose2:" << localRobot->getRobotNode("TCP R")->getGlobalPose();
- calculationRunning = true;
}
}
+ calculationRunning = true;
if (requested)
{
@@ -344,7 +348,7 @@ namespace armarx
{
//ARMARX_DEBUG << deactivateSpam(1) << "orientationVelocity before ChangeFrame: " << data.orientationVelocity->output();
data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
- ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
+ ARMARX_INFO << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
Eigen::Matrix3f rot;
rot = Eigen::AngleAxisf(data.orientationVelocity->z*cycleTime*0.001, Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(data.orientationVelocity->y*cycleTime*0.001, Eigen::Vector3f::UnitY())
@@ -668,7 +672,7 @@ namespace armarx
for (unsigned int i=0; i<nodes.size();i++)
{
- ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
+ ARMARX_INFO_S << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
{
@@ -684,7 +688,7 @@ namespace armarx
VectorXf newJointValues;
rns->getJointValues(newJointValues);
resultJointDelta = newJointValues - oldJointValues;
-// ARMARX_DEBUG << "joint angle deltas:\n" << dThetaSum;
+// ARMARX_INFO << "joint angle deltas:\n" << dThetaSum;
// check tolerances
if (checkTolerances())
@@ -696,12 +700,12 @@ namespace armarx
float d = dTheta.norm();
float posDist = getMeanErrorPosition();
float oriErr = getErrorRotation(rns->getTCP());
- if (dTheta.norm()<mininumChange)
- {
-// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
- break;
- }
+// if (dTheta.norm()<mininumChange)
+// {
+//// if (verbose)
+// ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << endl;
+// break;
+// }
if (checkImprovement && posDist>lastDist)
{
@@ -803,7 +807,7 @@ namespace armarx
// applyDOFWeightsToJacobian(Jacobian);
ARMARX_DEBUG << VAROUT(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
- ARMARX_DEBUG << VAROUT(Jacobian);
+ ARMARX_DEBUG << VAROUT(pseudo);
ARMARX_DEBUG << VAROUT(error);
return pseudo * error;
}
diff --git a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
index 7a6601400684e28806902cac3f9653d04a2de4ce..41d7d68b6116d476855e372273366e86755a0df2 100644
--- a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.cpp
@@ -24,10 +24,13 @@ HandUnitGuiPlugin::HandUnitGuiPlugin()
HandUnitWidget::HandUnitWidget() :
handName("NOT SET YET"),
- handUnitProxyName("")
+ handUnitProxyName(""),
+ setJointAnglesFlag(false)
{
// init gui
ui.setupUi(getWidget());
+
+ jointAngleUpdateTask = new PeriodicTask<HandUnitWidget>(this, &HandUnitWidget::setJointAngles, 50);
}
@@ -41,6 +44,8 @@ void HandUnitWidget::onInitComponent()
void HandUnitWidget::onConnectComponent()
{
connectSlots();
+ jointAngleUpdateTask->start();
+
handUnitProxy = getProxy<HandUnitInterfacePrx>(handUnitProxyName);
handName = handUnitProxy->getHandName();
@@ -61,9 +66,15 @@ void HandUnitWidget::onConnectComponent()
std::string shape = ((preshapeStrings->getVariant(i))->get<std::string>());
list << QString::fromStdString(shape);
}
+ ui.comboPreshapes->clear();
ui.comboPreshapes->addItems(list);
}
+void HandUnitWidget::onDisconnectComponent()
+{
+ jointAngleUpdateTask->stop();
+}
+
void HandUnitWidget::onExitComponent()
{
}
@@ -86,12 +97,23 @@ void HandUnitWidget::configured()
void HandUnitWidget::preshapeHand()
{
- ARMARX_INFO << "Setting new hand shape: " << ui.comboPreshapes->currentText().toUtf8().data();
- handUnitProxy->setShape(ui.comboPreshapes->currentText().toUtf8().data());
+ setPreshape(ui.comboPreshapes->currentText().toUtf8().data());
}
void HandUnitWidget::setJointAngles()
{
+ if(!handUnitProxy)
+ {
+ ARMARX_WARNING << "invalid proxy";
+ return;
+ }
+
+ if(!setJointAnglesFlag)
+ {
+ return;
+ }
+ setJointAnglesFlag = false;
+
NameValueMap ja;
if(handName == "Hand L" || handName == "TCP L")
@@ -132,6 +154,37 @@ void HandUnitWidget::setJointAngles()
handUnitProxy->setJointAngles(ja);
}
+void HandUnitWidget::requestSetJointAngles()
+{
+ setJointAnglesFlag = true;
+}
+
+void HandUnitWidget::openHand()
+{
+ setPreshape("Open");
+}
+
+void HandUnitWidget::closeHand()
+{
+ setPreshape("Close");
+}
+
+void HandUnitWidget::closeThumb()
+{
+ setPreshape("Thumb");
+}
+
+void HandUnitWidget::relaxHand()
+{
+ setPreshape("Relax");
+}
+
+void HandUnitWidget::setPreshape(std::string preshape)
+{
+ ARMARX_INFO << "Setting new hand shape: " << preshape;
+ handUnitProxy->setShape(preshape);
+}
+
void HandUnitWidget::loadSettings(QSettings *settings)
{
handUnitProxyName = settings->value("handUnitProxyName", QString::fromStdString(handUnitProxyName)).toString().toStdString();
@@ -147,11 +200,21 @@ void HandUnitWidget::saveSettings(QSettings *settings)
void HandUnitWidget::connectSlots()
{
- //connect(ui.buttonOpen, SIGNAL(clicked()), this, SLOT(openHand()));
- //connect(ui.buttonClose, SIGNAL(clicked()), this, SLOT(closeHand()));
connect(ui.buttonPreshape, SIGNAL(clicked()), this, SLOT(preshapeHand()));
+ connect(ui.buttonOpenHand, SIGNAL(clicked()), this, SLOT(openHand()));
+ connect(ui.buttonCloseHand, SIGNAL(clicked()), this, SLOT(closeHand()));
+ connect(ui.buttonCloseThumb, SIGNAL(clicked()), this, SLOT(closeThumb()));
+ connect(ui.buttonRelaxHand, SIGNAL(clicked()), this, SLOT(relaxHand()));
//connect(ui.comboPreshapes, SIGNAL(currentIndexChanged(const QString&)), this, SLOT(selectPreshape(const QString&)));
- connect(ui.buttonSetJointAngles, SIGNAL(clicked()), this, SLOT(setJointAngles()));
+ connect(ui.buttonSetJointAngles, SIGNAL(clicked()), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderIndexJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderIndexJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderMiddleJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderMiddleJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderRinky, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderPalm, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderThumbJ0, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
+ connect(ui.horizontalSliderThumbJ1, SIGNAL(sliderMoved(int)), this, SLOT(requestSetJointAngles()));
}
diff --git a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.h b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.h
index 20c38b2e46de5d70b10547a61cdbbb234eaf7125..5c3ac80b240f27214542111830c78ed145e4ad99 100644
--- a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.h
+++ b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.h
@@ -38,6 +38,8 @@
#include <string>
+#include <Core/core/services/tasks/PeriodicTask.h>
+
namespace armarx
{
@@ -87,6 +89,7 @@ namespace armarx
// inherited from Component
virtual void onInitComponent();
virtual void onConnectComponent();
+ virtual void onDisconnectComponent();
virtual void onExitComponent();
// HandUnitListener interface
@@ -107,6 +110,14 @@ namespace armarx
void preshapeHand();
void setJointAngles();
+ void requestSetJointAngles();
+ void openHand();
+ void closeHand();
+ void closeThumb();
+ void relaxHand();
+
+ private:
+ void setPreshape(std::string preshape);
protected:
void connectSlots();
@@ -123,6 +134,9 @@ namespace armarx
//QPointer<QWidget> __widget;
QPointer<HandUnitConfigDialog> dialog;
+ PeriodicTask<HandUnitWidget>::pointer_type jointAngleUpdateTask;
+ bool setJointAnglesFlag;
+
// HandUnitListener interface
public:
diff --git a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.ui b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
index 35eed4df97e671feb01ae2ffddc8c78e9ce4b616..bda4f4ae305f68c0b8b31a63850f6b8e79e07fdd 100644
--- a/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
+++ b/source/RobotAPI/gui_plugins/HandUnitPlugin/HandUnitGuiPlugin.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>518</width>
+ <width>637</width>
<height>396</height>
</rect>
</property>
@@ -20,7 +20,14 @@
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="2" column="1" colspan="2">
+ <item row="1" column="3">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1" colspan="7">
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
@@ -29,10 +36,27 @@
<enum>QFrame::Raised</enum>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0">
- <widget class="QLabel" name="label_2">
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_8">
<property name="text">
- <string>Thumb J0</string>
+ <string>Rinky</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>Thumb J1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QSlider" name="horizontalSliderThumbJ1">
+ <property name="maximum">
+ <number>90</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
@@ -46,6 +70,13 @@
</property>
</widget>
</item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>Middle J1</string>
+ </property>
+ </widget>
+ </item>
<item row="5" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
@@ -53,18 +84,15 @@
</property>
</widget>
</item>
- <item row="6" column="1">
- <widget class="QSlider" name="horizontalSliderMiddleJ1">
- <property name="maximum">
- <number>90</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Index J1</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QSlider" name="horizontalSliderPalm">
+ <item row="6" column="1">
+ <widget class="QSlider" name="horizontalSliderMiddleJ1">
<property name="maximum">
<number>90</number>
</property>
@@ -73,8 +101,8 @@
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QSlider" name="horizontalSliderThumbJ1">
+ <item row="5" column="1">
+ <widget class="QSlider" name="horizontalSliderMiddleJ0">
<property name="maximum">
<number>90</number>
</property>
@@ -83,8 +111,8 @@
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QSlider" name="horizontalSliderIndexJ1">
+ <item row="1" column="1">
+ <widget class="QSlider" name="horizontalSliderThumbJ0">
<property name="maximum">
<number>90</number>
</property>
@@ -93,22 +121,22 @@
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_5">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_2">
<property name="text">
- <string>Index J1</string>
+ <string>Thumb J0</string>
</property>
</widget>
</item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_8">
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_4">
<property name="text">
- <string>Rinky</string>
+ <string>Index J0</string>
</property>
</widget>
</item>
- <item row="5" column="1">
- <widget class="QSlider" name="horizontalSliderMiddleJ0">
+ <item row="0" column="1">
+ <widget class="QSlider" name="horizontalSliderPalm">
<property name="maximum">
<number>90</number>
</property>
@@ -117,20 +145,6 @@
</property>
</widget>
</item>
- <item row="6" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>Middle J1</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Index J0</string>
- </property>
- </widget>
- </item>
<item row="3" column="1">
<widget class="QSlider" name="horizontalSliderIndexJ0">
<property name="maximum">
@@ -141,15 +155,8 @@
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Palm</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QSlider" name="horizontalSliderThumbJ0">
+ <item row="4" column="1">
+ <widget class="QSlider" name="horizontalSliderIndexJ1">
<property name="maximum">
<number>90</number>
</property>
@@ -158,13 +165,6 @@
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>Thumb J1</string>
- </property>
- </widget>
- </item>
<item row="3" column="2" rowspan="2">
<widget class="QPushButton" name="buttonSetJointAngles">
<property name="maximumSize">
@@ -178,9 +178,30 @@
</property>
</widget>
</item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Palm</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
+ <item row="1" column="7">
+ <widget class="QPushButton" name="buttonRelaxHand">
+ <property name="text">
+ <string>Relax</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="5">
+ <widget class="QPushButton" name="buttonCloseThumb">
+ <property name="text">
+ <string>Thumb</string>
+ </property>
+ </widget>
+ </item>
<item row="1" column="1">
<widget class="QComboBox" name="comboPreshapes"/>
</item>
@@ -197,7 +218,21 @@
</property>
</widget>
</item>
- <item row="0" column="1" colspan="2">
+ <item row="1" column="6">
+ <widget class="QPushButton" name="buttonCloseHand">
+ <property name="text">
+ <string>Close</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QPushButton" name="buttonOpenHand">
+ <property name="text">
+ <string>Open</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1" colspan="7">
<widget class="QLabel" name="labelInfo">
<property name="maximumSize">
<size>
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/CMakeLists.txt b/source/RobotAPI/gui_plugins/HapticUnitPlugin/CMakeLists.txt
index 85ec3418868f863d69b838f78ce7d6c8ce566e13..9caeeee68f7beded8305dce337f2fa83c8c02904 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/CMakeLists.txt
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/CMakeLists.txt
@@ -15,17 +15,25 @@ include_directories(${Eigen3_INCLUDE_DIR})
include(${QT_USE_FILE})
-file(GLOB SOURCES HapticUnitGuiPlugin.cpp
- HapticUnitConfigDialog.cpp
- MatrixDisplayWidget.cpp)
-file(GLOB HEADERS HapticUnitGuiPlugin.h
- HapticUnitConfigDialog.h
- MatrixDisplayWidget.h)
+file(GLOB SOURCES
+ HapticUnitGuiPlugin.cpp
+ HapticUnitConfigDialog.cpp
+ MatrixDisplayWidget.cpp
+ MatrixDatafieldDisplayWidget.cpp
+)
+
+file(GLOB HEADERS
+ HapticUnitGuiPlugin.h
+ HapticUnitConfigDialog.h
+ MatrixDisplayWidget.h
+ MatrixDatafieldDisplayWidget.h
+)
set(GUI_MOC_HDRS
HapticUnitGuiPlugin.h
HapticUnitConfigDialog.h
MatrixDisplayWidget.h
+ MatrixDatafieldDisplayWidget.h
)
set(GUI_UIS
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index ff77ae283299e3c2a62e1d9f632443c0a9e1ed19..75a989ad1f3197ac935b6d8bd93fc937f7ff7248 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -34,7 +34,6 @@ HapticUnitWidget::HapticUnitWidget()
hapticObserverProxyName = "HapticUnitObserver";
updateTimer = new QTimer(this);
- layout = new QGridLayout(getWidget());
@@ -102,15 +101,15 @@ void HapticUnitWidget::configured()
void HapticUnitWidget::updateData()
{
- for(std::pair<std::string, MatrixDisplayWidget*> pair : matrixDisplays)
+ for(std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
{
- MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
+ //MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
std::string name = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "name"))->getString();
- float rate = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "rate"))->getFloat();
+ std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "device"))->getString();
+ //float rate = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "rate"))->getFloat();
TimestampVariantPtr timestamp = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "timestamp")))->get<TimestampVariant>();
- MatrixDisplayWidget* matrixDisplay = pair.second;
- matrixDisplay->setData(matrix->toEigen().cast<double>());
- matrixDisplay->setInfoOverlay(QString::fromStdString(pair.first) + ": " + QString::fromStdString(name) + " " + QString::fromStdString(timestamp->toTime().toDateTime()) + " " + QString::number(rate, 'g', 4) + " Frames/s");
+ MatrixDatafieldDisplayWidget* matrixDisplay = pair.second;
+ matrixDisplay->setInfoOverlay(QString::fromStdString(deviceName) + ": " + QString::fromStdString(name) + "\n" + QString::fromStdString(timestamp->toTime().toDateTime()));
matrixDisplay->invokeUpdate();
}
@@ -119,18 +118,19 @@ void HapticUnitWidget::updateData()
void HapticUnitWidget::onContextMenu(QPoint point)
{
QMenu* contextMenu = new QMenu(getWidget());
- MatrixDisplayWidget* matrixDisplay = qobject_cast<MatrixDisplayWidget*>(sender());
+ MatrixDatafieldDisplayWidget* matrixDisplay = qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
if(!matrixDisplay) return;
QAction* setDeviceTag = contextMenu->addAction(tr("Set Device Tag"));
QAction* action = contextMenu->exec(matrixDisplay->mapToGlobal(point));
- std::string deviceName = reverseMap.at(matrixDisplay);
+ std::string channelName = channelNameReverseMap.at(matrixDisplay);
+ std::string deviceName = deviceNameReverseMap.at(matrixDisplay);
if(action == setDeviceTag)
{
- std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, deviceName, "name"))->getString();
+ std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "name"))->getString();
bool ok;
QString newTag = QInputDialog::getText(getWidget(), tr("Set Device Tag"),
@@ -138,6 +138,7 @@ void HapticUnitWidget::onContextMenu(QPoint point)
QString::fromStdString(tag), &ok);
if (ok && !newTag.isEmpty())
{
+ ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": " << newTag.toStdString();
weissHapticUnit->setDeviceTag(deviceName, newTag.toStdString());
}
}
@@ -160,18 +161,19 @@ void HapticUnitWidget::connectSlots()
{
connect(this, SIGNAL(doUpdateDisplayWidgets()), SLOT(updateDisplayWidgets()), Qt::QueuedConnection);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateData()));
+ connect(ui.checkBoxOffsetFilter, SIGNAL(stateChanged(int)), this, SLOT(onCheckBoxOffsetFilterStateChanged(int)));
}
void HapticUnitWidget::createMatrixWidgets()
{
- ARMARX_LOG << "HapticUnitWidget::createMatrixWidgets()";
+ //ARMARX_LOG << "HapticUnitWidget::createMatrixWidgets()";
emit doUpdateDisplayWidgets();
}
void HapticUnitWidget::updateDisplayWidgets()
{
QLayoutItem *child;
- while ((child = layout->takeAt(0)) != 0) {
+ while ((child = ui.gridLayoutDisplay->takeAt(0)) != 0) {
delete child;
}
@@ -179,18 +181,28 @@ void HapticUnitWidget::updateDisplayWidgets()
ChannelRegistry channels = hapticObserverProxy->getAvailableChannels(false);
for(std::pair<std::string, ChannelRegistryEntry> pair : channels)
{
-
- ARMARX_LOG << pair.first;
- MatrixDisplayWidget* matrixDisplay = new MatrixDisplayWidget(getWidget());
+ std::string channelName = pair.first;
+ MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(new DatafieldRef(hapticObserverProxy, channelName, "matrix"), hapticObserverProxy, getWidget());
matrixDisplay->setRange(0, 4095);
matrixDisplay->setContextMenuPolicy(Qt::CustomContextMenu);
connect(matrixDisplay, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(onContextMenu(QPoint)));
matrixDisplays.insert(std::make_pair(pair.first, matrixDisplay));
- reverseMap.insert(std::make_pair(matrixDisplay, pair.first));
- layout->addWidget(matrixDisplay, 0, i);
+ std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "device"))->getString();
+ channelNameReverseMap.insert(std::make_pair(matrixDisplay, channelName));
+ deviceNameReverseMap.insert(std::make_pair(matrixDisplay, deviceName));
+ ui.gridLayoutDisplay->addWidget(matrixDisplay, 0, i);
i++;
}
}
+void HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
+{
+ //ARMARX_IMPORTANT << "onCheckBoxOffsetFilterToggled: " << state;
+ for(std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
+ {
+ pair.second->enableOffsetFilter(ui.checkBoxOffsetFilter->isChecked());
+ }
+}
+
Q_EXPORT_PLUGIN2(robotapi_gui_HapticUnitGuiPlugin, HapticUnitGuiPlugin)
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
index 77d8f64a940707b2542b3ae5eb97d1f4e14ce8f3..e4e215cafccb09061befac7f9ae4871d98713f35 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
@@ -39,7 +39,7 @@
#include <string>
#include <QLayout>
-#include "MatrixDisplayWidget.h"
+#include "MatrixDatafieldDisplayWidget.h"
namespace armarx
@@ -103,6 +103,7 @@ namespace armarx
private slots:
void updateDisplayWidgets();
+ void onCheckBoxOffsetFilterStateChanged(int state);
protected:
void connectSlots();
@@ -124,10 +125,9 @@ namespace armarx
QTimer* updateTimer;
- QGridLayout* layout;
-
- std::map<std::string, MatrixDisplayWidget*> matrixDisplays;
- std::map<MatrixDisplayWidget*, std::string> reverseMap;
+ std::map<std::string, MatrixDatafieldDisplayWidget*> matrixDisplays;
+ std::map<MatrixDatafieldDisplayWidget*, std::string> channelNameReverseMap;
+ std::map<MatrixDatafieldDisplayWidget*, std::string> deviceNameReverseMap;
};
//typedef boost::shared_ptr<HapticUnitWidget> HapticUnitGuiPluginPtr;
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.ui b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.ui
index 51d71fb847c535cabeb8d0444aaa11139030ad81..fca3ad93b092cf943002ae51bb86408db0ad92d7 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.ui
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.ui
@@ -19,6 +19,20 @@
<property name="windowTitle">
<string>Form</string>
</property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QCheckBox" name="checkBoxOffsetFilter">
+ <property name="text">
+ <string>Offset Filter</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QWidget" name="widgetDisplay" native="true">
+ <layout class="QGridLayout" name="gridLayoutDisplay"/>
+ </widget>
+ </item>
+ </layout>
</widget>
<resources/>
<connections/>
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..655237021a4aa994631d08be2a760feb7eea6ab1
--- /dev/null
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
@@ -0,0 +1,123 @@
+#include "MatrixDatafieldDisplayWidget.h"
+//#include "ui_MatrixDatafieldDisplayWidget.h"
+
+#include <QPainter>
+#include <pthread.h>
+#include <iostream>
+
+#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
+#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+
+using namespace std;
+using namespace armarx;
+
+void MatrixDatafieldDisplayWidget::updateRequested()
+{
+ mtx.lock();
+ this->data = matrixDatafieldOffsetFiltered->getDataField()->get<MatrixFloat>()->toEigen();
+ this->percentiles = percentilesDatafield->getDataField()->get<MatrixFloat>()->toVector();
+ mtx.unlock();
+ update();
+}
+
+MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget *parent) :
+ QWidget(parent)
+{
+ this->matrixDatafield = DatafieldRefPtr::dynamicCast(matrixDatafield);
+ this->observer = observer;
+ this->min = 0;
+ this->max = 1;
+ this->data = MatrixXf(1, 1);
+ this->data(0, 0) = 0;
+ enableOffsetFilter(false);
+ QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
+ QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)};
+ this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
+
+ //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
+ connect(this, SIGNAL(doUpdate()), SLOT(updateRequested()), Qt::QueuedConnection);
+}
+
+MatrixDatafieldDisplayWidget::~MatrixDatafieldDisplayWidget()
+{
+ //delete ui;
+}
+
+void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent *)
+{
+ mtx.lock();
+
+ int paddingBottom = 40;
+ QPainter painter(this);
+ painter.fillRect(rect(), QColor::fromRgb(0,0,0));
+ int matrixHeight = (height() - paddingBottom) * 8 / 10;
+ int percentilesHeight = (height() - paddingBottom) - matrixHeight;
+ drawMatrix(QRect(0, 0, width(), matrixHeight), painter);
+ drawPercentiles(QRect(0, matrixHeight, width() - 1, percentilesHeight), painter);
+
+ painter.setPen(QColor(Qt::GlobalColor::gray));
+ painter.setFont(QFont("Arial", 12));
+ painter.drawText(rect(), Qt::AlignBottom | Qt::AlignRight, infoOverlay);
+
+ mtx.unlock();
+}
+
+void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
+{
+ if(enabled)
+ {
+ this->matrixDatafieldOffsetFiltered = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
+ }
+ else
+ {
+ this->matrixDatafieldOffsetFiltered = this->matrixDatafield;
+ }
+ this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)), matrixDatafieldOffsetFiltered));
+}
+
+QColor MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
+{
+ value = (value - min) / (max - min) * (colors.size() - 1);
+ if(value < 0) return colors[0];
+ if(value >= colors.size() - 1) return colors[colors.size() - 1];
+ int i = (int)value;
+ float f2 = value - i;
+ float f1 = 1- f2;
+ QColor c1 = colors[i];
+ QColor c2 = colors[i + 1];
+ return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
+}
+
+void MatrixDatafieldDisplayWidget::drawMatrix(const QRect &target, QPainter &painter)
+{
+ int pixelSize = std::min(target.width() / data.cols(), target.height() / data.rows());
+ int dx = (target.width() - pixelSize * data.cols()) / 2;
+ int dy = (target.height() - pixelSize * data.rows()) / 2;
+ painter.setFont(QFont("Arial", 8));
+ painter.setPen(QColor(Qt::GlobalColor::gray));
+
+ for(int x = 0; x < data.cols(); x++)
+ {
+ for(int y = 0; y < data.rows(); y++)
+ {
+ QRect target = QRect(dx + x * pixelSize, dy + y * pixelSize, pixelSize, pixelSize);
+ painter.fillRect(target, getColor(data(y, x), min, max));
+ painter.drawText(target, Qt::AlignCenter, QString::number(data(y, x)));
+ }
+ }
+}
+
+void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect &target, QPainter &painter)
+{
+ painter.setPen(QColor(Qt::GlobalColor::gray));
+ painter.drawRect(target);
+ painter.setPen(QColor(Qt::GlobalColor::red));
+ for(int i = 0; i < (int)percentiles.size() - 1; i++)
+ {
+ int x1 = i * target.width() / (percentiles.size() - 1);
+ int x2 = (i + 1) * target.width() / (percentiles.size() - 1);
+ float y1 = (percentiles.at(i) - min) / (max - min) * target.height();
+ float y2 = (percentiles.at(i + 1) - min) / (max - min) * target.height();
+ painter.drawLine(target.left() + x1, target.bottom() - (int)y1, target.left() + x2, target.bottom() - (int)y2);
+ }
+}
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
new file mode 100644
index 0000000000000000000000000000000000000000..edc77d914906d094e59a2a89a28dc14f7ae33b75
--- /dev/null
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
@@ -0,0 +1,73 @@
+#ifndef MATRIXDISPLAYWIDGET_H
+#define MATRIXDISPLAYWIDGET_H
+
+#include <QWidget>
+#include <QMutex>
+#include <eigen3/Eigen/Dense>
+#include <valarray>
+#include <Core/interface/observers/ObserverInterface.h>
+#include <Core/observers/variant/DatafieldRef.h>
+
+
+using Eigen::MatrixXf;
+
+namespace Ui {
+class MatrixDatafieldDisplayWidget;
+}
+
+namespace armarx
+{
+ class MatrixDatafieldDisplayWidget : public QWidget
+ {
+ Q_OBJECT
+
+ signals:
+ void doUpdate();
+
+ private slots:
+ void updateRequested();
+
+ public:
+ explicit MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget *parent = 0);
+ ~MatrixDatafieldDisplayWidget();
+ void paintEvent(QPaintEvent *);
+
+
+ void setRange(float min, float max)
+ {
+ this->min = min;
+ this->max = max;
+ }
+ void enableOffsetFilter(bool enabled);
+ QColor getColor(float value, float min, float max);
+
+ void invokeUpdate(){
+ emit doUpdate();
+ }
+ void setInfoOverlay(QString infoOverlay)
+ {
+ mtx.lock();
+ this->infoOverlay = infoOverlay;
+ mtx.unlock();
+ }
+
+ private:
+ void drawMatrix(const QRect &target, QPainter &painter);
+ void drawPercentiles(const QRect &target, QPainter &painter);
+
+ private:
+ Ui::MatrixDatafieldDisplayWidget *ui;
+ MatrixXf data;
+ std::vector<float> percentiles;
+ float min, max;
+ std::valarray<QColor> colors;
+ QMutex mtx;
+ QString infoOverlay;
+ DatafieldRefPtr matrixDatafield;
+ DatafieldRefPtr matrixDatafieldOffsetFiltered;
+ DatafieldRefPtr percentilesDatafield;
+ ObserverInterfacePrx observer;
+ };
+}
+
+#endif // MATRIXDISPLAYWIDGET_H
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
index 102a023af65fb93cbd109abf85b02f1fb2e5c866..dc1124675d390a9ba43e9a57726cc3321e67f998 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
@@ -15,13 +15,13 @@ MatrixDisplayWidget::MatrixDisplayWidget(QWidget *parent) :
ui->setupUi(this);
this->min = 0;
this->max = 1;
- this->data = MatrixXd(1, 1);
+ this->data = MatrixXf(1, 1);
this->data(0, 0) = 0;
QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)};
this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
- //connect(this, SIGNAL(updateData(MatrixXd)), SLOT(setData(MatrixXd)), Qt::QueuedConnection);
+ //connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
connect(this, SIGNAL(doUpdate()), SLOT(update()), Qt::QueuedConnection);
}
@@ -33,7 +33,7 @@ MatrixDisplayWidget::~MatrixDisplayWidget()
void MatrixDisplayWidget::paintEvent(QPaintEvent *)
{
mtx.lock();
- MatrixXd data = this->data;
+ MatrixXf data = this->data;
//cout << "[" << pthread_self() << "] MatrixDisplayWidget::paintEvent" << endl;
@@ -59,14 +59,14 @@ void MatrixDisplayWidget::paintEvent(QPaintEvent *)
mtx.unlock();
}
-QColor MatrixDisplayWidget::getColor(double value, double min, double max)
+QColor MatrixDisplayWidget::getColor(float value, float min, float max)
{
value = (value - min) / (max - min) * (colors.size() - 1);
if(value < 0) return colors[0];
if(value >= colors.size() - 1) return colors[colors.size() - 1];
int i = (int)value;
- double f2 = value - i;
- double f1 = 1- f2;
+ float f2 = value - i;
+ float f1 = 1- f2;
QColor c1 = colors[i];
QColor c2 = colors[i + 1];
return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.h b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.h
index 2978adac43164649a7cb317d1b4201d7c738b022..33480b6a827ba490812895638a9ca7febca3124c 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.h
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/MatrixDisplayWidget.h
@@ -6,7 +6,7 @@
#include <eigen3/Eigen/Dense>
#include <valarray>
-using Eigen::MatrixXd;
+using Eigen::MatrixXf;
namespace Ui {
class MatrixDisplayWidget;
@@ -22,7 +22,7 @@ namespace armarx
void doUpdate();
public slots:
- void setData(MatrixXd data)
+ void setData(MatrixXf data)
{
mtx.lock();
this->data = data;
@@ -37,12 +37,12 @@ namespace armarx
- void setRange(double min, double max)
+ void setRange(float min, float max)
{
this->min = min;
this->max = max;
}
- QColor getColor(double value, double min, double max);
+ QColor getColor(float value, float min, float max);
void invokeUpdate(){
emit doUpdate();
@@ -56,8 +56,8 @@ namespace armarx
private:
Ui::MatrixDisplayWidget *ui;
- MatrixXd data;
- double min, max;
+ MatrixXf data;
+ float min, max;
std::valarray<QColor> colors;
QMutex mtx;
QString infoOverlay;
diff --git a/source/RobotAPI/interface/observers/ObserverFilters.ice b/source/RobotAPI/interface/observers/ObserverFilters.ice
index 02dee61eb985df72a78dd9f73674aad77966bae0..ed2ef8b3f7fa1253619036e7e2c4cb86a29195a0 100644
--- a/source/RobotAPI/interface/observers/ObserverFilters.ice
+++ b/source/RobotAPI/interface/observers/ObserverFilters.ice
@@ -43,21 +43,35 @@ module armarx
};
["cpp:virtual"]
- class MatrixMaxFilterBase extends DatafieldFilterBase
+ class MatrixMinFilterBase extends DatafieldFilterBase
{
};
["cpp:virtual"]
- class MatrixMinFilterBase extends DatafieldFilterBase
+ class MatrixAvgFilterBase extends DatafieldFilterBase
{
};
["cpp:virtual"]
- class MatrixAvgFilterBase extends DatafieldFilterBase
+ class MatrixPercentileFilterBase extends DatafieldFilterBase
{
+ float percentile;
+ };
+ ["cpp:virtual"]
+ class MatrixPercentilesFilterBase extends DatafieldFilterBase
+ {
+ int percentiles;
+ };
+
+ ["cpp:virtual"]
+ class MatrixCumulativeFrequencyFilterBase extends DatafieldFilterBase
+ {
+ float min;
+ float max;
+ int bins;
};
};
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index 70a8707b3836c23daa287a710719d028783badea..fdb7d914cf9e07b0430e7f77692f26c8d5045fbe 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -174,6 +174,9 @@ namespace armarx
add<armarx::MatrixMaxFilterBase, armarx::filters::MatrixMaxFilter>(map);
add<armarx::MatrixMinFilterBase, armarx::filters::MatrixMinFilter>(map);
add<armarx::MatrixAvgFilterBase, armarx::filters::MatrixAvgFilter>(map);
+ add<armarx::MatrixPercentileFilterBase, armarx::filters::MatrixPercentileFilter>(map);
+ add<armarx::MatrixPercentilesFilterBase, armarx::filters::MatrixPercentilesFilter>(map);
+ add<armarx::MatrixCumulativeFrequencyFilterBase, armarx::filters::MatrixCumulativeFrequencyFilter>(map);
return map;
}
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index ba5cf8ce90c279e80085353869c8a3ba25a9693b..801e0c24b90a3a914369e90b9cd7de180ef6a6f5 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -27,7 +27,7 @@
#include <Core/observers/filters/DatafieldFilter.h>
#include <RobotAPI/interface/observers/ObserverFilters.h>
#include <Core/util/variants/eigen3/MatrixVariant.h>
-
+#include <algorithm>
namespace armarx
{
@@ -39,10 +39,17 @@ namespace armarx
public DatafieldFilter
{
public:
- MatrixMaxFilter() { }
+ MatrixMaxFilter()
+ {
+ this->windowFilterSize = 1;
+ }
VariantBasePtr calculate(const Ice::Current &) const
{
+ if(dataHistory.size() == 0)
+ {
+ return new Variant(new MatrixFloat(1, 1));
+ }
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().maxCoeff());
@@ -60,10 +67,17 @@ namespace armarx
public DatafieldFilter
{
public:
- MatrixMinFilter() { }
+ MatrixMinFilter()
+ {
+ this->windowFilterSize = 1;
+ }
VariantBasePtr calculate(const Ice::Current &) const
{
+ if(dataHistory.size() == 0)
+ {
+ return new Variant(new MatrixFloat(1, 1));
+ }
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().minCoeff());
@@ -81,10 +95,17 @@ namespace armarx
public DatafieldFilter
{
public:
- MatrixAvgFilter() { }
+ MatrixAvgFilter()
+ {
+ this->windowFilterSize = 1;
+ }
VariantBasePtr calculate(const Ice::Current &) const
{
+ if(dataHistory.size() == 0)
+ {
+ return new Variant(new MatrixFloat(1, 1));
+ }
VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().mean());
@@ -97,6 +118,167 @@ namespace armarx
}
};
+ class MatrixPercentileFilter :
+ public MatrixPercentileFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixPercentileFilter()
+ {
+ this->windowFilterSize = 1;
+ }
+ MatrixPercentileFilter(float percentile)
+ {
+ this->percentile = percentile;
+ this->windowFilterSize = 1;
+ }
+
+ VariantBasePtr calculate(const Ice::Current &) const
+ {
+ if(dataHistory.size() == 0)
+ {
+ return new Variant(new MatrixFloat(1, 1));
+ }
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ return new Variant(GetPercentile(vector, percentile));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+
+ static float GetPercentile(const std::vector<float> &sortedData, float percentile)
+ {
+ if(sortedData.size() == 0)
+ {
+ throw LocalException("GetPercentile not possible for empty vector");
+ }
+ float indexf = (sortedData.size() - 1) * percentile;
+ indexf = std::max(0.f, std::min(sortedData.size() - 1.f, indexf));
+ int index = (int)indexf;
+ float f = indexf - index;
+ if(index == (int)sortedData.size() - 1)
+ {
+ return sortedData.at(sortedData.size() - 1);
+ }
+ return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
+ }
+ };
+
+ class MatrixPercentilesFilter :
+ public MatrixPercentilesFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixPercentilesFilter()
+ {
+ this->windowFilterSize = 1;
+ this->percentiles = 10;
+ }
+ MatrixPercentilesFilter(int percentiles)
+ {
+ this->percentiles = percentiles;
+ this->windowFilterSize = 1;
+ }
+
+ VariantBasePtr calculate(const Ice::Current &) const
+ {
+ if(dataHistory.size() == 0)
+ {
+ ARMARX_IMPORTANT_S << "no data";
+ return new Variant(new MatrixFloat(1, 1));
+ }
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ std::vector<float> result;
+ result.push_back(vector.at(0));
+ for(int i = 1; i < percentiles; i++)
+ {
+ result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
+ }
+ result.push_back(vector.at(vector.size() - 1));
+ return new Variant(new MatrixFloat(1, result.size(), result));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ };
+
+ class MatrixCumulativeFrequencyFilter :
+ public MatrixCumulativeFrequencyFilterBase,
+ public DatafieldFilter
+ {
+ public:
+ MatrixCumulativeFrequencyFilter()
+ {
+ this->windowFilterSize = 1;
+ }
+ MatrixCumulativeFrequencyFilter(float min, float max, int bins)
+ {
+ this->min = min;
+ this->max = max;
+ this->bins = bins;
+ this->windowFilterSize = 1;
+ }
+ VariantBasePtr calculate(const Ice::Current &) const
+ {
+ if(dataHistory.size() == 0)
+ {
+ return new Variant(new MatrixFloat(1, 1));
+ }
+ VariantPtr currentValue = VariantPtr::dynamicCast(dataHistory.rbegin()->second);
+ MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ std::vector<float> vector = matrix->toVector();
+ std::sort(vector.begin(), vector.end());
+ std::vector<int> result = Calculate(vector, min, max, bins);
+ std::vector<float> resultF;
+ for(int v : result)
+ {
+ resultF.push_back(v);
+ }
+
+ return new Variant(new MatrixFloat(1, resultF.size(), resultF));
+ }
+ ParameterTypeList getSupportedTypes(const Ice::Current &) const
+ {
+ ParameterTypeList result;
+ result.push_back(VariantType::MatrixFloat);
+ return result;
+ }
+ static std::vector<int> Calculate(const std::vector<float> &sortedData, float min, float max, int bins)
+ {
+ std::vector<int> result;
+ float val = min;
+ int nr = 0;
+ int lastCount = 0;
+ for(size_t i = 0; i < sortedData.size(); i++)
+ {
+ if(sortedData.at(i) > val && nr < bins)
+ {
+ result.push_back(i);
+ nr++;
+ val = min + (max - min) * nr / bins;
+ lastCount = i;
+ }
+ }
+ while((int)result.size() < bins)
+ {
+ result.push_back(lastCount);
+ }
+ return result;
+ }
+
+ };
}
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/CMakeLists.txt b/source/RobotAPI/libraries/drivers/WeissHapticSensor/CMakeLists.txt
index ea86aacfe1fcc31c23923373a307a0a4fbb5a040..69c5f8a2b3968cf42865d9755d9ee15c585dd829 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/CMakeLists.txt
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/CMakeLists.txt
@@ -16,7 +16,7 @@ set(LIB_SOVERSION 0)
set(LIBS RobotAPIUnits ArmarXCoreObservers ArmarXCoreEigen3Variants)
set(LIB_FILES
- WeissHapticSensorsUnit.cpp
+ WeissHapticUnit.cpp
WeissHapticSensor.cpp
AbstractInterface.cpp
BinaryLogger.cpp
@@ -28,7 +28,7 @@ set(LIB_FILES
CalibrationHelper.cpp
)
set(LIB_HEADERS
- WeissHapticSensorsUnit.h
+ WeissHapticUnit.h
WeissHapticSensor.h
AbstractInterface.h
BinaryLogger.h
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 9bd5cf3dae8beb73974b7d9126ebc7dabe1d679e..32b44ac4749e7ad76dae442406d97fef4211889c 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -5,8 +5,8 @@
using namespace armarx;
-WeissHapticSensor::WeissHapticSensor(std::string device)
- : device(device), connected(false), setDeviceTagScheduled(false)
+WeissHapticSensor::WeissHapticSensor(std::string device, int minimumReportIntervalMs)
+ : device(device), connected(false), setDeviceTagScheduled(false), minimumReportIntervalMs(minimumReportIntervalMs)
{
sensorTask = new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
boost::smatch match;
@@ -17,9 +17,9 @@ WeissHapticSensor::WeissHapticSensor(std::string device)
void WeissHapticSensor::connect()
{
- //cout << "Open Serial" << endl;
+ ARMARX_INFO << "Open Serial" << endl;
this->interface.reset(new SerialInterface(device.c_str(), 115200));
- interface->startLogging(deviceFileName + ".transmission.log");
+ //interface->startLogging(deviceFileName + ".transmission.log");
interface->open();
//cout << *interface << endl;
this->sensor.reset(new TactileSensor(interface));
@@ -66,11 +66,13 @@ void WeissHapticSensor::connect()
sensor->setAquisitionWindow(1, 1, mi.res_x, mi.res_y);
sensor->setFrontEndGain(255);
+ ARMARX_LOG << "[" << device << "] Front end gain set to " << (int)sensor->getFrontEndGain();
- ARMARX_LOG << "Front end gain set to " << (int)sensor->getFrontEndGain();
+ sensor->setThreshold(0);
+ ARMARX_LOG << "[" << device << "] threshold set to " << (int)sensor->getThreshold();
connected = true;
- cout << this << ": Connect done, Interface=" << sensor->getInterfaceInfo() << endl;
+ ARMARX_LOG << device << ": Connect done, Interface=" << sensor->getInterfaceInfo();
}
void WeissHapticSensor::disconnect()
@@ -86,7 +88,7 @@ void WeissHapticSensor::setListenerPrx(HapticUnitListenerPrx listenerPrx)
void WeissHapticSensor::startSampling()
{
- cout << this << ": startSampling" << endl;
+ ARMARX_LOG << device << ": startSampling" << endl;
sensorTask->start();
}
@@ -104,13 +106,18 @@ void WeissHapticSensor::scheduleSetDeviceTag(string tag)
void WeissHapticSensor::frameAcquisitionTaskLoop()
{
- cout << this << ": readAndReportSensorValues" << endl;
+ ARMARX_LOG << device << ": readAndReportSensorValues";
//bool periodic = false;
- cout << sensor->getInterfaceInfo() << endl;
+ ARMARX_LOG << device << ": Interface Info: " << sensor->getInterfaceInfo();
- cout << this << ": startPeriodicFrameAcquisition" << endl;
+ ARMARX_LOG << device << this << ": startPeriodicFrameAcquisition";
sensor->startPeriodicFrameAcquisition(0);
+ IceUtil::Time lastFrameTime = IceUtil::Time::now();
+
+ math::SlidingWindowVectorMedian slidingMedian(mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
+
+
while(!sensorTask->isStopped())
{
//ARMARX_INFO << deactivateSpam(1) << this << ": receicePeriodicFrame";
@@ -122,6 +129,25 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
//long end = TimestampVariant::nowLong();
//cout << end - start << endl;
+ std::vector<float> sensorValues;
+ for(int i = 0; i < mi.res_x * mi.res_y; i++)
+ {
+ sensorValues.push_back(data.data->at(i));
+ }
+ slidingMedian.addEntry(sensorValues);
+
+ MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
+ std::vector<float> filteredSensorValues = slidingMedian.getMedian();
+ for (int y = 0; y < mi.res_y; y++)
+ {
+ for (int x = 0; x < mi.res_x; x++)
+ {
+ (*matrix)(y, x) = filteredSensorValues.at(y * mi.res_x + x);
+ }
+ }
+
+
+ /*
MatrixFloatPtr matrix = new MatrixFloat(mi.res_y, mi.res_x);
for (int y = 0; y < mi.res_y; y++)
{
@@ -131,10 +157,19 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
(*matrix)(y, x) = val;
}
}
+ */
- TimestampVariantPtr now = TimestampVariant::nowPtr();
- writeMatrixToJs(matrix, now);
- listenerPrx->reportSensorValues(device, tag, matrix, now);
+ IceUtil::Time now = IceUtil::Time::now();
+
+ TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+ writeMatrixToJs(matrix, nowTimestamp);
+
+ IceUtil::Time interval = now - lastFrameTime;
+ if(interval.toMilliSeconds() >= minimumReportIntervalMs)
+ {
+ listenerPrx->reportSensorValues(device, tag, matrix, nowTimestamp);
+ lastFrameTime = now;
+ }
}
catch(ChecksumErrorException)
{
@@ -149,7 +184,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
ARMARX_INFO << "[" << device << "] Stopping periodic frame aquisition to set new device tag";
sensor->stopPeriodicFrameAcquisition();
- ARMARX_INFO << "[" << device << "] Setting new device tag '" << setDeviceTagValue << "'";
+ ARMARX_IMPORTANT << "[" << device << "] Setting new device tag '" << setDeviceTagValue << "'";
sensor->setDeviceTag(setDeviceTagValue);
this->tag = setDeviceTagValue;
@@ -162,7 +197,7 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
//usleep(1000000);
}
- cout << this << ": stopPeriodicFrameAcquisition" << endl;
+ ARMARX_LOG << device << ": stopPeriodicFrameAcquisition";
sensor->stopPeriodicFrameAcquisition();
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
index 44eb078d0c08d15a9fff0fa421553178f1b045a1..884c946e7ee9632bc4f8f8f4a63d13eb3bdb70f9 100644
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -10,6 +10,7 @@
//#include <Core/util/variants/eigen3/Eigen3LibRegistry.h>
#include "TextWriter.h"
#include <Core/observers/variant/TimestampVariant.h>
+#include <Core/util/math/SlidingWindowVectorMedian.h>
#include <Core/util/variants/eigen3/MatrixVariant.h>
#include <boost/thread/mutex.hpp>
@@ -22,7 +23,7 @@ namespace armarx
class WeissHapticSensor : public Logging
{
public:
- WeissHapticSensor(std::string device);
+ WeissHapticSensor(std::string device, int minimumReportIntervalMs);
void connect();
void disconnect();
@@ -51,6 +52,7 @@ namespace armarx
std::string setDeviceTagValue;
boost::mutex mutex;
+ int minimumReportIntervalMs;
};
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.cpp b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
old mode 100755
new mode 100644
similarity index 53%
rename from source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.cpp
rename to source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
index 1c7085f5b15176f75e10fb685e5864bcb703423b..24c1edda1132ec2290eba62c5ca4252e5a5906dc
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.cpp
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.cpp
@@ -21,7 +21,7 @@
* GNU General Public License
*/
-#include "WeissHapticSensorsUnit.h"
+#include "WeissHapticUnit.h"
#include <boost/regex.hpp>
#include <boost/filesystem.hpp>
@@ -29,54 +29,19 @@
using namespace armarx;
-void WeissHapticSensorsUnit::onInitHapticUnit()
+void WeissHapticUnit::onInitHapticUnit()
{
- // Read pipe Id property
- /*int firstPipeID = getProperty<int>("FirstPipeID").getValue();
- if(firstPipeID < 0)
- {
- throw UserException("Value of FirstPipeID must be bigger than 0");
- }*/
-
- // Read pipe base name property
- // std::string pipeBaseName = getProperty<std::string>("PipeBaseName").getValue();
-
- // Read logfile property
- //std::string logfile = getProperty<std::string>("Logfile").getValue();
-
- //std::vector<std::string> sensors;
- //sensors.push_back("RightSole.HapticMatrix");
- //hapticProtocol.defineSensorCategory(250, sensors);
-
- // TODO: The task frequency needs to be adapted when the underlying sensor frequency changes
- //sensorTask = new RunningTask<WeissHapticSensorsUnit>(this, &WeissHapticSensorsUnit::readAndReportSensorValues);
-
- //remoteSystemReady = false;
- /*
- if(!hapticProtocol.openPipes(firstPipeID, firstPipeID + 1, false, true, pipeBaseName))
- {
- throw UserException("Failed to open pipes");
- }
-
- if(!hapticProtocol.openLogfile(logfile))
- {
- throw UserException("Failed to open logfile");
- }*/
std::vector<std::string> devices = getDevices();
- /*boost::shared_ptr<WeissHapticSensor> sensor(new WeissHapticSensor(devices.front()));
- this->sensors.push_back(sensor);*/
for(std::vector<std::string>::iterator it = devices.begin(); it != devices.end(); ++it)
{
- WeissHapticSensorPtr sensor(new WeissHapticSensor(*it));
+ WeissHapticSensorPtr sensor(new WeissHapticSensor(*it, 20)); // minimumReportIntervalMs = 20, limit to maximum 50 frames/s
this->sensors.push_back(sensor);
}
std::cout << "Connect Interfaces" << std::endl;
- //sensors.front()->connect();
-
for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
{
(*it)->connect();
@@ -84,7 +49,7 @@ void WeissHapticSensorsUnit::onInitHapticUnit()
}
-std::vector< std::string > WeissHapticSensorsUnit::getDevices()
+std::vector< std::string > WeissHapticUnit::getDevices()
{
const std::string target_path( "/dev/" );
const boost::regex my_filter( "ttyACM[0-9]+" );
@@ -108,37 +73,47 @@ std::vector< std::string > WeissHapticSensorsUnit::getDevices()
}
std::sort(files.begin(), files.end());
- std::cout << "Detected ACM-Interfaces: " << std::endl;
- for(std::vector<std::string>::iterator it = files.begin(); it != files.end(); ++it)
+ if(files.size() == 0)
{
- std::cout << *it << std::endl;
+ ARMARX_WARNING << "No ACM-Interfaces found";
+ }
+ else
+ {
+ ARMARX_INFO << "Detected ACM-Interfaces: " << files.size();
+ for(std::string file : files)
+ {
+ ARMARX_INFO << "Found device: " << file;
+ }
}
return files;
}
-void WeissHapticSensorsUnit::setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &)
+void WeissHapticUnit::setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &)
{
for(WeissHapticSensorPtr sensor : sensors)
{
if(sensor->getDeviceName() == deviceName)
{
+ ARMARX_IMPORTANT << "scheduling to set new device tag for " << deviceName << ": " << tag;
sensor->scheduleSetDeviceTag(tag);
+ return;
}
}
+ ARMARX_WARNING << "device not found: " << deviceName;
}
-void WeissHapticSensorsUnit::startLogging(const Ice::Current &)
+void WeissHapticUnit::startLogging(const Ice::Current &)
{
// @@@ TODO NotImplemented
}
-void WeissHapticSensorsUnit::stopLogging(const Ice::Current &)
+void WeissHapticUnit::stopLogging(const Ice::Current &)
{
// @@@ TODO NotImplemented
}
-void WeissHapticSensorsUnit::onStartHapticUnit()
+void WeissHapticUnit::onStartHapticUnit()
{
for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
@@ -149,7 +124,7 @@ void WeissHapticSensorsUnit::onStartHapticUnit()
}
-void WeissHapticSensorsUnit::onExitHapticUnit()
+void WeissHapticUnit::onExitHapticUnit()
{
for(std::vector<boost::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
{
@@ -157,18 +132,18 @@ void WeissHapticSensorsUnit::onExitHapticUnit()
}
}
-/*void WeissHapticSensorsUnit::onConnectComponent()
+/*void WeissHapticUnit::onConnectComponent()
{
}*/
-void WeissHapticSensorsUnit::onDisconnectComponent()
+void WeissHapticUnit::onDisconnectComponent()
{
}
-PropertyDefinitionsPtr WeissHapticSensorsUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr WeissHapticUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new WeissHapticSensorsUnitPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new WeissHapticUnitPropertyDefinitions(getConfigIdentifier()));
}
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.h b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
old mode 100755
new mode 100644
similarity index 89%
rename from source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.h
rename to source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
index 1a0f955718658b8760e56366896ec657da47dc1e..ff24168d0b939d42bb3e48755ba206e9331827fb
--- a/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.h
+++ b/source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticUnit.h
@@ -33,21 +33,21 @@
namespace armarx
{
- class WeissHapticSensorsUnitPropertyDefinitions : public HapticUnitPropertyDefinitions
+ class WeissHapticUnitPropertyDefinitions : public HapticUnitPropertyDefinitions
{
public:
- WeissHapticSensorsUnitPropertyDefinitions(std::string prefix):
+ WeissHapticUnitPropertyDefinitions(std::string prefix):
HapticUnitPropertyDefinitions(prefix)
{
}
};
- class WeissHapticSensorsUnit :
+ class WeissHapticUnit :
virtual public WeissHapticUnitInterface,
virtual public HapticUnit
{
public:
- virtual std::string getDefaultName() { return "WeissHapticSensorsUnit"; }
+ virtual std::string getDefaultName() { return "WeissHapticUnit"; }
virtual void onInitHapticUnit();
virtual void onStartHapticUnit();
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/um_wts0614-34_04-2013_en.pdf b/source/RobotAPI/libraries/drivers/WeissHapticSensor/um_wts0614-34_04-2013_en.pdf
new file mode 100755
index 0000000000000000000000000000000000000000..73181a124343bfd66c6f5e94c81300b4db698f7e
Binary files /dev/null and b/source/RobotAPI/libraries/drivers/WeissHapticSensor/um_wts0614-34_04-2013_en.pdf differ
diff --git a/source/RobotAPI/libraries/drivers/WeissHapticSensor/wts_command_set_reference_manual.pdf b/source/RobotAPI/libraries/drivers/WeissHapticSensor/wts_command_set_reference_manual.pdf
new file mode 100755
index 0000000000000000000000000000000000000000..9b259fb180986e1f48da7151511f028cd53af457
Binary files /dev/null and b/source/RobotAPI/libraries/drivers/WeissHapticSensor/wts_command_set_reference_manual.pdf differ
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index ef16919a0b64586cfe6fffc0d42ffd55bcccfe1a..17fa472e974f7c24c540105e8aab23fa842e6191 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -1,2 +1,4 @@
add_subdirectory(operations)
+
+add_subdirectory(WeissHapticGroup)
\ No newline at end of file
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt b/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1bb8a3e825f4e4af96ddd3666d76d9b5a21094fc
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/CMakeLists.txt
@@ -0,0 +1,48 @@
+armarx_component_set_name("WeissHapticGroup")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+#
+# all include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# include_directories(${MyLib_INCLUDE_DIRS})
+#endif()
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
+
+
+if (Eigen3_FOUND AND Simox_FOUND)
+ include_directories(
+ ${Eigen3_INCLUDE_DIR}
+ ${Simox_INCLUDE_DIRS}
+ )
+endif()
+
+set(COMPONENT_LIBS
+ RobotAPIInterfaces RobotAPICore RobotAPIUnits
+ ArmarXInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
+
+# Sources
+
+set(SOURCES
+WeissHapticGroupRemoteStateOfferer.cpp
+./WeissHapticSensorTest.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+
+set(HEADERS
+WeissHapticGroupRemoteStateOfferer.h
+WeissHapticGroupStatechartContext.generated.h
+./WeissHapticSensorTest.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+./WeissHapticSensorTest.generated.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.generated.h
+)
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..d030a3d328389657a297a2573b5b900ec46d84fe
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroup.scgxml
@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="WeissHapticGroup" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPICore.hapticObserver"/>
+ <Proxy value="RobotAPICore.weissHapticUnit"/>
+ </Proxies>
+ <State filename="WeissHapticSensorTest.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a8f0c6b0e2bb73a32a26da3ba4a23afef190c40c
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp
@@ -0,0 +1,66 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::WeissHapticGroup::WeissHapticGroupRemoteStateOfferer
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "WeissHapticGroupRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace WeissHapticGroup;
+
+// DO NOT EDIT NEXT LINE
+WeissHapticGroupRemoteStateOfferer::SubClassRegistry WeissHapticGroupRemoteStateOfferer::Registry(WeissHapticGroupRemoteStateOfferer::GetName(), &WeissHapticGroupRemoteStateOfferer::CreateInstance);
+
+
+
+WeissHapticGroupRemoteStateOfferer::WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > (reader)
+{
+}
+
+void WeissHapticGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void WeissHapticGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void WeissHapticGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string WeissHapticGroupRemoteStateOfferer::GetName()
+{
+ return "WeissHapticGroupRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr WeissHapticGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+ return XMLStateOffererFactoryBasePtr(new WeissHapticGroupRemoteStateOfferer(reader));
+}
+
+
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..232ed4f3261a52cfb2ea1d2b6c3d3479a488bc74
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::WeissHapticGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_REMOTESTATEOFFERER_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_REMOTESTATEOFFERER_H
+
+#include <Core/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "WeissHapticGroupStatechartContext.generated.h"
+
+namespace armarx
+{
+ namespace WeissHapticGroup
+ {
+ class WeissHapticGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer();
+ void onConnectXMLRemoteStateOfferer();
+ void onExitXMLRemoteStateOfferer();
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
new file mode 100644
index 0000000000000000000000000000000000000000..947ae7ed0391362ec7e483b4003c67748d58e4b3
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupStatechartContext.generated.h
@@ -0,0 +1,59 @@
+#ifndef ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
+#define ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
+
+#include <Core/core/Component.h>
+#include <Core/core/system/ImportExportComponent.h>
+#include <Core/statechart/StatechartContext.h>
+#include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/interface/units/WeissHapticUnit.h>
+
+namespace armarx
+{
+ namespace WeissHapticGroup
+ {
+ class WeissHapticGroupStatechartContext
+ : virtual public StatechartContext
+ {
+ protected:
+ class PropertyDefinitions
+ : public StatechartContextPropertyDefinitions
+ {
+ public:
+ PropertyDefinitions(std::string prefix)
+ : StatechartContextPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("HapticUnitObserverName", "HapticUnitObserver", "Name of the haptic unit observer that should be used");
+ defineOptionalProperty<std::string>("WeissHapticUnitName", "WeissHapticUnit", "Name of the weiss haptic unit that should be used");
+ }
+ }; // class PropertyDefinitions
+
+ private:
+ HapticUnitObserverInterfacePrx hapticObserver;
+ WeissHapticUnitInterfacePrx weissHapticUnit;
+
+ public:
+ std::string getDefaultName() const
+ {
+ return "WeissHapticGroupStatechartContext";
+ }
+ virtual void onInitStatechartContext()
+ {
+ usingProxy(getProperty<std::string>("HapticUnitObserverName").getValue());
+ usingProxy(getProperty<std::string>("WeissHapticUnitName").getValue());
+ }
+ virtual void onConnectStatechartContext()
+ {
+ hapticObserver = getProxy<HapticUnitObserverInterfacePrx>(getProperty<std::string>("HapticUnitObserverName").getValue());
+ weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(getProperty<std::string>("WeissHapticUnitName").getValue());
+ }
+ HapticUnitObserverInterfacePrx getHapticObserver() const { return hapticObserver; }
+ WeissHapticUnitInterfacePrx getWeissHapticUnit() const { return weissHapticUnit; }
+ virtual PropertyDefinitionsPtr createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new WeissHapticGroupStatechartContext::PropertyDefinitions(getConfigIdentifier()));
+ }
+ }; // class WeissHapticGroupStatechartContext
+ } // namespace WeissHapticGroup
+} // namespace armarx
+
+#endif // ARMARX_COMPONENT_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICGROUPSTATECHARTCONTEXT_H
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac0edd94540c762281d997bea5684cc08bf3fb58
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
@@ -0,0 +1,112 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::WeissHapticGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "WeissHapticSensorTest.h"
+
+#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
+#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+
+using namespace armarx;
+using namespace WeissHapticGroup;
+
+// DO NOT EDIT NEXT LINE
+WeissHapticSensorTest::SubClassRegistry WeissHapticSensorTest::Registry(WeissHapticSensorTest::GetName(), &WeissHapticSensorTest::CreateInstance);
+
+
+
+WeissHapticSensorTest::WeissHapticSensorTest(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<WeissHapticSensorTest>(stateData), WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>(stateData)
+{
+}
+
+void WeissHapticSensorTest::onEnter()
+{
+ WeissHapticGroupStatechartContext* context = getContext<WeissHapticGroupStatechartContext>();
+ HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
+ ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
+ std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap;
+ if(channels.size() == 0)
+ {
+ ARMARX_WARNING << "No tactile pads found";
+ }
+ else
+ {
+ ARMARX_INFO << "Creating tactile channels";
+ for(std::pair<std::string, ChannelRegistryEntry> pair : channels)
+ {
+ std::string tactilePad = pair.first;
+ DatafieldRefBasePtr matrixDatafield = new DatafieldRef(hapticObserver, tactilePad, "matrix");
+ DatafieldRefBasePtr matrixNulled = hapticObserver->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield);
+ DatafieldRefBasePtr matrixMax = hapticObserver->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixMaxFilter()), matrixNulled);
+ tactileDatafields_MaximumValueMap.insert(std::make_pair(tactilePad, DatafieldRefPtr::dynamicCast(matrixMax)));
+ }
+ }
+ local.setTactileDatafields_MaximumValue(tactileDatafields_MaximumValueMap);
+
+
+}
+
+void WeissHapticSensorTest::run()
+{
+ std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap = local.getTactileDatafields_MaximumValue();
+
+ while (!isRunningTaskStopped()) // stop run function if returning true
+ {
+ std::stringstream ss;
+ std::stringstream ssNames;
+ int max = 0;
+ for(std::pair<std::string, DatafieldRefPtr> pair : tactileDatafields_MaximumValueMap)
+ {
+ std::string tactilePad = pair.first;
+ DatafieldRefPtr matrixMax = pair.second;
+ int padMax = (int)matrixMax->getDataField()->getFloat();
+ ss << padMax << "; ";
+ ssNames << tactilePad << "; ";
+ max = std::max(max, padMax);
+ }
+ ARMARX_IMPORTANT << "tactile max value: " << max << "; \n\n" << ss.str() << "\n" << ssNames.str();
+
+ usleep(10000); // 100ms
+ }
+
+}
+
+void WeissHapticSensorTest::onBreak()
+{
+ // put your user code for the breaking point here
+ // execution time should be short (<100ms)
+}
+
+void WeissHapticSensorTest::onExit()
+{
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr WeissHapticSensorTest::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new WeissHapticSensorTest(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
new file mode 100644
index 0000000000000000000000000000000000000000..a4cd708d5e6efbb5207bf86e3fe58518bb58d65e
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.generated.h
@@ -0,0 +1,120 @@
+#ifndef _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
+#define _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
+
+#include <Core/statechart/xmlstates/XMLState.h>
+#include "WeissHapticGroupStatechartContext.generated.h"
+#include <Core/observers/ObserverObjectFactories.h>
+#include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/interface/units/WeissHapticUnit.h>
+
+namespace armarx
+{
+ namespace WeissHapticGroup
+ {
+ template<typename StateType>
+ class WeissHapticSensorTestGeneratedBase
+ : virtual public XMLStateTemplate < StateType >,
+ public XMLStateFactoryBase
+ {
+ protected:
+ class WeissHapticSensorTestIn
+ {
+ private:
+ WeissHapticSensorTestGeneratedBase<StateType> *parent;
+
+ public:
+ WeissHapticSensorTestIn(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ : parent(parent)
+ {
+ }
+ }; // class WeissHapticSensorTestIn
+
+ class WeissHapticSensorTestLocal
+ {
+ private:
+ WeissHapticSensorTestGeneratedBase<StateType> *parent;
+
+ public:
+ WeissHapticSensorTestLocal(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ : parent(parent)
+ {
+ }
+
+ public:
+ std::map<std::string, ::armarx::DatafieldRefPtr> getTactileDatafields_MaximumValue() const
+ {
+ return parent->State::getLocal< ::armarx::StringValueMap>("TactileDatafields_MaximumValue")->::armarx::StringValueMap::toStdMap< ::armarx::DatafieldRefPtr>();
+ }
+ void setTactileDatafields_MaximumValue(const std::map<std::string, ::armarx::DatafieldRefPtr> & value) const
+ {
+ ::armarx::StringValueMapPtr container = ::armarx::StringValueMap::FromStdMap< ::armarx::DatafieldRefPtr>(value);
+ parent->State::setLocal("TactileDatafields_MaximumValue", *container);
+ }
+ bool isTactileDatafields_MaximumValueSet() const
+ {
+ return parent->State::isLocalParameterSet("TactileDatafields_MaximumValue");
+ }
+ }; // class WeissHapticSensorTestLocal
+
+ class WeissHapticSensorTestOut
+ {
+ private:
+ WeissHapticSensorTestGeneratedBase<StateType> *parent;
+
+ public:
+ WeissHapticSensorTestOut(WeissHapticSensorTestGeneratedBase<StateType> *parent)
+ : parent(parent)
+ {
+ }
+ }; // class WeissHapticSensorTestOut
+
+ protected:
+ const WeissHapticSensorTestIn in;
+ const WeissHapticSensorTestLocal local;
+ const WeissHapticSensorTestOut out;
+
+ public:
+ WeissHapticSensorTestGeneratedBase(const XMLStateConstructorParams& stateData)
+ : XMLStateTemplate < StateType > (stateData),
+ in(WeissHapticSensorTestIn(this)),
+ local(WeissHapticSensorTestLocal(this)),
+ out(WeissHapticSensorTestOut(this))
+ {
+ }
+ WeissHapticSensorTestGeneratedBase(const WeissHapticSensorTestGeneratedBase &source)
+ : IceUtil::Shared(source),
+ armarx::StateIceBase(source),
+ armarx::StateBase(source),
+ armarx::StateController(source),
+ armarx::State(source),
+ XMLStateTemplate < StateType > (source),
+ in(WeissHapticSensorTestIn(this)),
+ local(WeissHapticSensorTestLocal(this)),
+ out(WeissHapticSensorTestOut(this))
+ {
+ }
+
+ public:
+ HapticUnitObserverInterfacePrx getHapticObserver() const
+ {
+ return StateBase::getContext<WeissHapticGroupStatechartContext>()->getHapticObserver();
+ }
+ WeissHapticUnitInterfacePrx getWeissHapticUnit() const
+ {
+ return StateBase::getContext<WeissHapticGroupStatechartContext>()->getWeissHapticUnit();
+ }
+ static std::string GetName()
+ {
+ return "WeissHapticSensorTest";
+ }
+ void __forceLibLoading()
+ {
+ // Do not call this method.
+ // The sole purpose of this method is to force the compiler/linker to include all libraries.
+ ::armarx::DatafieldRef type1;
+ }
+ }; // class WeissHapticSensorTestGeneratedBase
+ } // namespace WeissHapticGroup
+} // namespace armarx
+
+#endif // _ARMARX_XMLUSERCODE_ARMARX_WEISSHAPTICGROUP_WEISSHAPTICSENSORTESTGENERATEDBASE_H
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
new file mode 100644
index 0000000000000000000000000000000000000000..34b84f98978965e66af0c0f79a2c9322ca177df3
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::WeissHapticGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2015
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_WeissHapticSensorTest_H
+#define _ARMARX_XMLUSERCODE_RobotAPI_WeissHapticGroup_WeissHapticSensorTest_H
+
+#include "WeissHapticSensorTest.generated.h"
+
+namespace armarx
+{
+ namespace WeissHapticGroup
+ {
+ class WeissHapticSensorTest :
+ public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
+ {
+ public:
+ WeissHapticSensorTest(const XMLStateConstructorParams& stateData);
+
+ // inherited from StateBase
+ void onEnter();
+ void run();
+ void onBreak();
+ void onExit();
+
+ // static functions for AbstractFactory Method
+ static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
+ static SubClassRegistry Registry;
+
+ // DO NOT INSERT ANY CLASS MEMBERS,
+ // use stateparameters instead,
+ // if classmember are neccessary nonetheless, reset them in onEnter
+ };
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d44dc26ac48b7524c6ef53fcff2728aa6496aa64
--- /dev/null
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.0" name="WeissHapticSensorTest" uuid="B8841E3D-97C6-468F-86D9-6370D5F89BB4" width="800" height="600">
+ <InputParameters/>
+ <OutputParameters/>
+ <LocalParameters>
+ <Parameter name="TactileDatafields_MaximumValue" type="::armarx::StringValueMapBase(::armarx::DatafieldRefBase)" optional="no"/>
+ </LocalParameters>
+ <Substates/>
+ <Events/>
+ <Transitions/>
+</State>
+