diff --git a/scenarios/MovePlatformToLandmarkTest/WidgetMovePlatformToLandmarkApp.armarxgui b/scenarios/MovePlatformToLandmarkTest/WidgetMovePlatformToLandmarkApp.armarxgui
new file mode 100644
index 0000000000000000000000000000000000000000..81cb8bad1c5d5a2a8e029d180844ba465940dfc7
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/WidgetMovePlatformToLandmarkApp.armarxgui
@@ -0,0 +1,38 @@
+[General]
+loadedLibs=Gui/build/lib/libConditionViewerGuiPlugin.so, Gui/build/lib/libHandUnitGuiPlugin.so, Gui/build/lib/libHardwareGuiPlugin.so, Gui/build/lib/libKinematicUnitGuiPlugin.so, Gui/build/lib/libLoggingGuiPlugin.so, Gui/build/lib/libObjectExaminerGuiPlugin.so, Gui/build/lib/libObserverPropertiesGuiPlugin.so, Gui/build/lib/libPlatformUnitGuiPlugin.so, Gui/build/lib/libSensorActorWidgetsGuiPlugin.so, Gui/build/lib/libStateChartGuiPlugin.so, Gui/build/lib/libSystemStateMonitorGuiPlugin.so, SimulationX/build/lib/libSimulatorControlGuiPlugin.so
+MainWindowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\xff\xff\xff\xff\0\0\0\x18\0\0\a\x80\0\0\x4 \0\0\0\0\0\0\0\0\xff\xff\xff\xfe\xff\xff\xff\xfe\0\0\0\0\x2\0)
+DockWidgetsState="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x1\0\0\0\x1\0\0\a\x80\0\0\x3\xbd\xfc\x2\0\0\0\x2\xfc\0\0\0\x19\0\0\x3\x2\0\0\0\xdc\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfb\0\0\0\x1a\0\x44\0o\0\x63\0k\0L\0o\0g\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0\0\0\0\x5\xdd\0\0\0\xc8\0\xff\xff\xff\xfb\0\0\0,\0\x44\0o\0\x63\0k\0S\0y\0s\0t\0\x65\0m\0S\0t\0\x61\0t\0\x65\0M\0o\0n\0i\0t\0o\0r\x1\0\0\x5\xe3\0\0\x1\x9d\0\0\0\xd3\0\xff\xff\xff\xfb\0\0\0\x1e\0\x44\0o\0\x63\0k\0\x45\0v\0\x65\0n\0t\0S\0\x65\0n\0\x64\0\x65\0r\x1\0\0\x3!\0\0\0\xb5\0\0\0\xaa\0\xff\xff\xff\0\0\0\0\0\0\x3\xbd\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0)"
+WidgetCustomNames=EventSender, LogViewer, SystemStateMonitor
+
+[EventSender]
+WidgetBaseName=EventSender
+widgetWidth=1920
+widgetHeight=161
+EventSender\1\eventSenderName=startMovePlatformToLandmarkTest
+EventSender\1\description=
+EventSender\1\componentName=RobotControlStateOfferer
+EventSender\1\globalStateIdentifier=RobotStatechart->RobotControl->Functional
+EventSender\1\eventName=EvLoadScenario
+EventSender\1\eventReceiverName=toAll
+EventSender\1\parameters\1\key=proxyName
+EventSender\1\parameters\1\VariantTypeId=-2125418521
+EventSender\1\parameters\1\value=MovePlatformToLandmarkStateChartStateOfferer
+EventSender\1\parameters\2\key=stateName
+EventSender\1\parameters\2\VariantTypeId=-2125418521
+EventSender\1\parameters\2\value=MovePlatformToLandmarkStateChart
+EventSender\1\parameters\size=2
+EventSender\size=1
+
+[LogViewer]
+WidgetBaseName=LogViewer
+widgetWidth=1501
+widgetHeight=750
+verbosityLevel=1
+autoFilterAdding=true
+
+[SystemStateMonitor]
+WidgetBaseName=SystemStateMonitor
+widgetWidth=413
+widgetHeight=750
+ManagerRepository=@Invalid()
+MonitoredManagers=ArmarXGuiManager, ArmarXSimulatorManager, CommonStorageManager, ConditionHandlerManager, KinematicUnitDynamicSimulationManager, KinematicUnitObserverManager, KinematicUnitSimulationManager, LongtermMemoryManager, MovePlatformAppManager, PlatformUnitDynamicSimulationManager, PlatformUnitObserverManager, PriorKnowledgeManager, RobotControlManager, RobotStateComponentManager, SystemObserverManager, WavingManager
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/ArmarXGui.cfg b/scenarios/MovePlatformToLandmarkTest/configs/ArmarXGui.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..72e2e2ed2fb8fed63738266f09fe8ab9994bc0a3
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/ArmarXGui.cfg
@@ -0,0 +1,7 @@
+ArmarX.LoadPlugins=${ArmarXHome_DIR}/Gui/build/lib/libKinematicUnitGuiPlugin.so
+ArmarX.Show3DViewer=yes
+# model XML file path containing a VirtualRobot RobotNodeSet that defines the joints
+ArmarX.KinematicUnitGuiPlugin.RobotFileName=Armar4/data/robotmodel/ArmarIV.xml
+# node set name
+ArmarX.KinematicUnitGuiPlugin.RobotNodeSetName=Robot
+ArmarX.KinematicUnitGuiPlugin.KinematicUnitName=Armar4KinematicUnit
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/ConditionHandler.cfg b/scenarios/MovePlatformToLandmarkTest/configs/ConditionHandler.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..3bb2beb8145f198ecfbdd70005c24fe002562c4f
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/ConditionHandler.cfg
@@ -0,0 +1,92 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: .cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = .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.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/MovePlatformToLandmarkTest/configs/Global.cfg b/scenarios/MovePlatformToLandmarkTest/configs/Global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..77927624693f052359958908ce6d8a1bb0e6bc4f
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/Global.cfg
@@ -0,0 +1,5 @@
+ArmarX.DisableLogging = no
+VisionX.DisableLogging = no
+
+ArmarX.GlobalMinimumLoggingLevel = Verbose
+VisionX.GlobalMinimumLoggingLevel = Verbose
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExample.xml b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExample.xml
new file mode 100644
index 0000000000000000000000000000000000000000..57f2c211283dc7499d812259fbbe80643b1b8824
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExample.xml
@@ -0,0 +1,45 @@
+<MovePlatformToLandmarkStateChart>
+ <StateParameters>
+ <!--
+ x/y are in mm
+ for now z=rotation around z axis in radian (values from about -PI to +PI)
+ -->
+ <targetLandmark>Test1</targetLandmark>
+ <landmarkTags>
+ <Item0>Start</Item0>
+ <Item1>Test1</Item1>
+ </landmarkTags>
+ <!-- lists of lists seem impossible, hence why there's only one point per landmark-->
+ <landmarkPositions>
+ <Item0>
+ <x>0</x>
+ <y>0</y>
+ <z>0</z>
+ </Item0>
+ <Item1>
+ <x>0</x>
+ <y>10000</y>
+ <z>0</z>
+ </Item1>
+ <Item2>
+ <x>0</x>
+ <y>10000</y>
+ <z>1</z>
+ </Item2>
+ <Item3>
+ <x>-3000</x>
+ <y>5000</y>
+ <z>1</z>
+ </Item3>
+ <!--Item4>
+ <x>0</x>
+ <y>0</y>
+ <z>0</z>
+ </Item4-->
+ </landmarkPositions>
+ <positionalAccuracy>11</positionalAccuracy> <!--mm-->
+ <orientationalAccuracy>0.2</orientationalAccuracy> <!--rad-->
+ <timeoutMoveTo>30001</timeoutMoveTo>
+ </StateParameters>
+</MovePlatformToLandmarkStateChart>
+
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml
new file mode 100644
index 0000000000000000000000000000000000000000..012ecdb702733fefb65b1c7b9f3233c6e7d5375d
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml
@@ -0,0 +1,87 @@
+<MovePlatformToLandmarkStateChart>
+ <StateParameters>
+ <!--
+ x/y are in mm
+ for now z=rotation around z axis in radian (values from about -PI to +PI)
+ -->
+ <targetLandmark>handover</targetLandmark>
+ <landmarkNodes>
+ <Item0>
+ <x>0</x>
+ <y>0</y>
+ <z>0</z>
+ <frame>start</frame>
+ </Item0>
+ <Item1>
+ <x>4200</x>
+ <y>0</y>
+ <z>0</z>
+ <frame>1</frame>
+ </Item1>
+ <Item2>
+ <x>4200</x>
+ <y>4000</y>
+ <z>0</z>
+ <frame>2</frame>
+ </Item2>
+ <Item3>
+ <x>4200</x>
+ <y>4000</y>
+ <z>3</z>
+ <frame>handover</frame>
+ </Item3>
+ <Item4>
+ <x>3100</x>
+ <y>6000</y>
+ <z>0</z>
+ <frame>3</frame>
+ </Item4>
+ <Item5>
+ <x>3300</x>
+ <y>8300</y>
+ <z>0</z>
+ <frame>4</frame>
+ </Item5>
+ <Item6>
+ <x>2600</x>
+ <y>7600</y>
+ <z>0.8</z>
+ <frame>oven</frame>
+ </Item6>
+ <Item7>
+ <x>3300</x>
+ <y>7150</y>
+ <z>-1.6</z>
+ <frame>table</frame>
+ </Item7>
+ <Item8>
+ <x>2600</x>
+ <y>9600</y>
+ <z>0.1</z>
+ <frame>sink</frame>
+ </Item8>
+ <Item9>
+ <x>4500</x>
+ <y>9400</y>
+ <z>0</z>
+ <frame>cupboard</frame>
+ </Item9>
+ </landmarkNodes>
+ <landmarkEdges>
+ <Item0>start-1</Item0>
+ <Item1>1-2</Item1>
+ <Item2>2-handover</Item2>
+ <Item3>2-3</Item3>
+ <Item4>3-4</Item4>
+ <Item5>3-oven</Item5>
+ <Item6>3-table</Item6>
+ <Item7>4-oven</Item7>
+ <Item8>4-table</Item8>
+ <Item9>4-sink</Item9>
+ <Item10>4-cupboard</Item10>
+ </landmarkEdges>
+ <positionalAccuracy>10</positionalAccuracy> <!--mm-->
+ <orientationalAccuracy>0.1</orientationalAccuracy> <!--rad-->
+ <timeoutMoveTo>30000</timeoutMoveTo>
+ </StateParameters>
+</MovePlatformToLandmarkStateChart>
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkTest.cfg b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkTest.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..972396a37efb0d9b6ea4823dc6c269f9876b21bf
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkTest.cfg
@@ -0,0 +1,2 @@
+ArmarX.MovePlatformToLandmarkStateChartStateOfferer.KinematicUnitObserverName=PlatformUnitObserver
+ArmarX.MovePlatformToLandmarkStateChartStateOfferer.KinematicUnitName=PlatformUnitDynamicSimulation
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/PlatformUnitObserver.cfg b/scenarios/MovePlatformToLandmarkTest/configs/PlatformUnitObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5fafd2950db0ef2b16a91dcb0c7996564af6e4c8
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/PlatformUnitObserver.cfg
@@ -0,0 +1,62 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: .cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = .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.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.PlatformUnitObserver properties
+# ==================================================================
+
+ArmarX.PlatformUnitObserver.PlatformName=Platform
+
+
+
diff --git a/scenarios/MovePlatformToLandmarkTest/configs/RobotControl.cfg b/scenarios/MovePlatformToLandmarkTest/configs/RobotControl.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..a5a0610eddf235a4011b5fdb0eb4370ed621ae0a
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/RobotControl.cfg
@@ -0,0 +1,76 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: .cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = .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.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ==================================================================
+# ArmarX.RobotControlStateOfferer properties
+# ==================================================================
+
+# 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/MovePlatformToLandmarkTest/configs/SystemObserver.cfg b/scenarios/MovePlatformToLandmarkTest/configs/SystemObserver.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..1347203c6bbb6d49bef0b13e1031aee2ca1e0155
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/configs/SystemObserver.cfg
@@ -0,0 +1,76 @@
+# ==================================================================
+# ArmarX properties
+# ==================================================================
+
+# ArmarX.CachePath: Path for cache files
+# Attributes:
+# - Default: .cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = .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.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/MovePlatformToLandmarkTest/startScenario.sh b/scenarios/MovePlatformToLandmarkTest/startScenario.sh
new file mode 100755
index 0000000000000000000000000000000000000000..a5d62311e83556d5b7d65c99b1eb4ebb5c55ffca
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/startScenario.sh
@@ -0,0 +1,24 @@
+
+export CORE_PATH=../../../Core
+export GUI_PATH=../../../Gui
+export VISIONX_PATH=../..././VisionX
+export ARMAR4_PATH=../../../Armar4
+export HUMANOIDROBOTAPI_PATH=../../../RobotAPI
+export SCRIPT_PATH=$CORE_PATH/build/bin
+export SIMULATION_PATH=../../../SimulationX
+
+export CORE_BIN_PATH=$CORE_PATH/build/bin
+export GUI_BIN_PATH=$GUI_PATH/build/bin
+export VISIONX_BIN_PATH=$VISIONX_PATH/build/bin
+export ARMAR4_BIN_PATH=$ARMAR4_PATH/build/bin
+export HUMANOIDROBOTAPI_BIN_PATH=$HUMANOIDROBOTAPI_PATH/build/bin
+
+export GLOBAL_CONFIG=./configs/Global.cfg
+
+# armarx components
+$SCRIPT_PATH/startApplication.sh $CORE_BIN_PATH/PlatformUnitObserverRun --Ice.Config=$GLOBAL_CONFIG,./configs/PlatformUnitObserver.cfg &
+$SCRIPT_PATH/startApplication.sh $CORE_BIN_PATH/SystemObserverRun --Ice.Config=$GLOBAL_CONFIG &
+$SCRIPT_PATH/startApplication.sh $CORE_BIN_PATH/RobotControlRun --Ice.Config=$GLOBAL_CONFIG &
+$SCRIPT_PATH/startApplication.sh $CORE_BIN_PATH/ConditionHandlerRun --Ice.Config=$GLOBAL_CONFIG &
+$SCRIPT_PATH/startApplication.sh $HUMANOIDROBOTAPI_BIN_PATH/MovePlatformAppRun --Ice.Config=$GLOBAL_CONFIG &
+$SCRIPT_PATH/startApplication.sh $HUMANOIDROBOTAPI_BIN_PATH/MovePlatformToLandmarkAppRun --Ice.Config=$GLOBAL_CONFIG,./configs/MovePlatformToLandmarkTest.cfg &
diff --git a/scenarios/MovePlatformToLandmarkTest/start_the_scenario_in_SimulatorGui_first b/scenarios/MovePlatformToLandmarkTest/start_the_scenario_in_SimulatorGui_first
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scenarios/MovePlatformToLandmarkTest/stopScenario.sh b/scenarios/MovePlatformToLandmarkTest/stopScenario.sh
new file mode 100755
index 0000000000000000000000000000000000000000..dfe89bbb34cdbb8bce7fda179e3ac7bf66723cd9
--- /dev/null
+++ b/scenarios/MovePlatformToLandmarkTest/stopScenario.sh
@@ -0,0 +1,32 @@
+
+skill ()
+{
+ STR=`ps aux | grep -re "$1 --Ice.Default" | grep -v grep | awk '{print $2}'`
+ if [ ${#STR} == 0 ]
+ then
+ echo skipping $1
+ else
+ echo killing $1
+ kill -s $2 `ps aux | grep -re "$1 --Ice.Default" | grep -v grep | awk '{print $2}'`
+ fi
+}
+
+echo SENDING SIGINT TO ALL PROCESSES
+skill PlatformUnitObserverRun 2
+skill SystemObserverRun 2
+skill ConditionHandlerRun 2
+skill RobotControlRun 2
+
+skill MovePlatformAppRun 2
+skill MovePlatformToLandmarkAppRun 2
+
+sleep 1
+
+echo SENDING SIGKILL TO ALL PROCESSES
+skill PlatformUnitObserverRun 9
+skill SystemObserverRun 9
+skill ConditionHandlerRun 9
+skill RobotControlRun 9
+
+skill MovePlatformAppRun 9
+skill MovePlatformToLandmarkAppRun 9
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index b0e4dc610e6a98332b4ae91b0c0bdfb8ced7d0d3..19d00fb8bbe9521f72140d25260980f17651f4c8 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(ForceTorqueObserver)
add_subdirectory(MotionControlTest)
add_subdirectory(TCPControlUnit)
add_subdirectory(MovePlatform)
+add_subdirectory(MovePlatformToLandmark)
diff --git a/source/RobotAPI/applications/MovePlatform/MovePlatformApp.h b/source/RobotAPI/applications/MovePlatform/MovePlatformApp.h
index cb18907adc4d1cbfa3a2c98f7af806ad8b83709e..a2ab3498da2695b08b25d70501b7f48308bafbd4 100644
--- a/source/RobotAPI/applications/MovePlatform/MovePlatformApp.h
+++ b/source/RobotAPI/applications/MovePlatform/MovePlatformApp.h
@@ -14,7 +14,7 @@
* 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::applications::MovePlatform
+ * @package RobotAPI::applications::MovePlatformApp
* @author Valerij Wittenbeck
* @date 2014
* @copyright http://www.gnu.org/licenses/gpl.txt
diff --git a/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt b/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..187ae9481a1e679f0b52ec30e92fd949fec3ccab
--- /dev/null
+++ b/source/RobotAPI/applications/MovePlatformToLandmark/CMakeLists.txt
@@ -0,0 +1,31 @@
+#test case to move platform to prespecified positions
+
+armarx_component_set_name(MovePlatformToLandmarkApp)
+
+find_package(Eigen3 QUIET)
+find_package(Simox QUIET)
+
+armarx_build_if(Eigen3_FOUND "Eigen3 not available")
+armarx_build_if(Simox_FOUND "Simox not available")
+#armarx_build_if(MemoryX_FOUND "VisionX not available")
+
+include_directories(${Eigen3_INCLUDE_DIR} ${Simox_INCLUDE_DIRS})
+
+set(COMPONENT_LIBS
+ #MovePlatform
+ MovePlatformToLandmark
+ ArmarXInterfaces
+ ArmarXCore
+ ArmarXCoreStatechart
+ ArmarXCoreObservers
+ ArmarXCoreOperations
+ ArmarXCoreRemoteRobot
+ #MemoryXInterfaces
+ #MemoryXCore
+ #MemoryXMemoryTypes
+ ${Simox_LIBRARIES})
+
+set(SOURCES main.cpp
+ MovePlatformToLandmarkApp.h)
+
+armarx_add_component_executable("${SOURCES}")
diff --git a/source/RobotAPI/applications/MovePlatformToLandmark/MovePlatformToLandmarkApp.h b/source/RobotAPI/applications/MovePlatformToLandmark/MovePlatformToLandmarkApp.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1b3bb43de2d22fa9e3ef35a3365e3e191e2e730
--- /dev/null
+++ b/source/RobotAPI/applications/MovePlatformToLandmark/MovePlatformToLandmarkApp.h
@@ -0,0 +1,48 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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::applications::MovePlatformToLandmarkApp
+ * @author Valerij Wittenbeck
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_APPLICATION_MovePlatformToLandmark_H
+#define _ARMARX_APPLICATION_MovePlatformToLandmark_H
+
+
+#include <RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.h>
+
+#include <Core/core/application/Application.h>
+
+namespace armarx
+{
+ class MovePlatformToLandmarkApp :
+ virtual public armarx::Application
+ {
+ /**
+ * @see armarx::Application::setup()
+ */
+ void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties)
+ {
+ registry->addObject( Component::create<MovePlatformToLandmarkStateChart>(properties) );
+ }
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/applications/MovePlatformToLandmark/main.cpp b/source/RobotAPI/applications/MovePlatformToLandmark/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a73e3d2957973db8f59d620089a7b14cec03f6c6
--- /dev/null
+++ b/source/RobotAPI/applications/MovePlatformToLandmark/main.cpp
@@ -0,0 +1,33 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * 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 Lesser 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::application::MovePlatformToLandmark
+ * @author Valerij Wittenbeck
+ * @date 2014
+ * @copyright http://www.gnu.org/licenses/gpl.txt
+ * GNU General Public License
+ */
+
+#include "MovePlatformToLandmarkApp.h"
+#include <Core/core/logging/Logging.h>
+
+int main(int argc, char* argv[])
+{
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::MovePlatformToLandmarkApp>();
+ app->setName("MovePlatformToLandmarkApp");
+
+ return app->main(argc, argv);
+}
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index 825e2e6bc0f94d29d2b5258732a0420fdbce5606..bcb3ae3b2041a18fbf5f7d1571e4b8669caa8899 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -1,3 +1,4 @@
add_subdirectory(GraspingWithTorques)
add_subdirectory(OpenHand)
add_subdirectory(MovePlatform)
+add_subdirectory(MovePlatformToLandmark)
diff --git a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
index 208bc51e5673d0da97c408aac3e3f68c83368238..d0d48e1bb3b35bdf4c6a3c3f959776d49b59da93 100644
--- a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
+++ b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.cpp
@@ -52,7 +52,6 @@ namespace armarx
void StatechartMovePlatform::defineParameters()
{
setConfigFile("RobotAPI/scenarios/MovePlatformTest/configs/MovePlatformExample.xml");
-
addToInput("targetPositions", VariantType::List(VariantType::Vector3), false);
addToInput("positionalAccuracy", VariantType::Float, false);
addToInput("orientationalAccuracy", VariantType::Float, false);
@@ -62,6 +61,7 @@ namespace armarx
void StatechartMovePlatform::defineSubstates()
{
+ ARMARX_LOG << eVERBOSE << "Entering StatechartMovePlatform";
StatePtr stateMoveToNext = addState<StateMoveToNext>("stateMoveToNext");
StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
StatePtr stateFailure = addState<FailureState>("stateFailure");
diff --git a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.h b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.h
index b63fd58c295eee8d9a991e03716d15c97992a7fa..b3bb67b012e9514f7ee5c6d165e16a9cf27ba6c4 100644
--- a/source/RobotAPI/statecharts/MovePlatform/MovePlatform.h
+++ b/source/RobotAPI/statecharts/MovePlatform/MovePlatform.h
@@ -14,9 +14,9 @@
* 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::GraspingWithTorques
-* @author Markus Przybylski
-* @date 2014 Markus Przybylski
+* @package RobotAPI::MovePlatform
+* @author Valerij Wittenbeck
+* @date 2014 Valerij Wittenbeck
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
diff --git a/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h b/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
index f30d54ce523d7a1dd0d07fa39db9d96eb5373483..aa69d53dace4e5304ec530efebe309486eba7668 100644
--- a/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
+++ b/source/RobotAPI/statecharts/MovePlatform/PlatformContext.h
@@ -102,9 +102,10 @@ namespace armarx
PlatformUnitObserverInterfacePrx platformUnitObserverPrx;
//SystemObserverInterfacePrx systemObserver; // already defined in StatechartContext
VirtualRobot::RobotPtr remoteRobot;
- private:
std::string platformUnitObserverName;
std::string platformUnitDynamicSimulationName;
+ private:
+
};
}
diff --git a/source/RobotAPI/statecharts/MovePlatformToLandmark/CMakeLists.txt b/source/RobotAPI/statecharts/MovePlatformToLandmark/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..77d5af0b2fc47e5a9d2f877270f52de2144058c2
--- /dev/null
+++ b/source/RobotAPI/statecharts/MovePlatformToLandmark/CMakeLists.txt
@@ -0,0 +1,28 @@
+armarx_set_target("RobotAPI Library: MovePlatformToLandmark")
+
+
+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(LIB_NAME MovePlatformToLandmark)
+set(LIB_VERSION 0.1.0)
+set(LIB_SOVERSION 0)
+
+set(LIBS RobotAPICore RobotAPIInterfaces ArmarXInterfaces ArmarXCore ArmarXCoreObservers)
+
+set(LIB_FILES MovePlatformToLandmark.cpp
+ )
+set(LIB_HEADERS
+ MovePlatformToLandmark.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a13fd60acf1ad9b8c620e6750de06dfd44692bce
--- /dev/null
+++ b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.cpp
@@ -0,0 +1,279 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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::MovePlatformToLandmark
+* @author Valerij Wittenbeck
+* @date 2014
+
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#include "MovePlatformToLandmark.h"
+//#include "../core/RobotStatechartContext.h"
+
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/variant/SingleTypeVariantList.h>
+#include <Core/robotstate/remote/ArmarPose.h>
+#include <Core/core/ManagedIceObject.h>
+#include <Core/robotstate/remote/RobotStateObjectFactories.h>
+#include <Core/core/CoreObjectFactories.h>
+#include <Core/observers/ObserverObjectFactories.h>
+#include <Core/interface/units/PlatformUnitInterface.h>
+
+#include <boost/algorithm/string.hpp>
+
+namespace armarx
+{
+
+
+ void MovePlatformToLandmarkStateChart::onInitRemoteStateOfferer()
+ {
+ addState<StatechartMovePlatformToLandmark>("MovePlatformToLandmarkStateChart");
+ }
+
+ void MovePlatformToLandmarkStateChart::onConnectRemoteStateOfferer()
+ {
+
+ }
+
+ // ****************************************************************
+ // Implementation of StatechartMovePlatformToLandmark
+ // ****************************************************************
+ void StatechartMovePlatformToLandmark::defineParameters()
+ {
+ setConfigFile("RobotAPI/scenarios/MovePlatformToLandmarkTest/configs/MovePlatformToLandmarkExampleGraph.xml");
+
+ addToInput("targetLandmark", VariantType::String, false);
+ addToInput("landmarkNodes", VariantType::List(VariantType::FramedVector3), false);
+ addToInput("landmarkEdges", VariantType::List(VariantType::String), false);
+ addToInput("positionalAccuracy", VariantType::Float, true);
+ addToInput("orientationalAccuracy", VariantType::Float, true);
+ addToInput("timeoutMoveTo", VariantType::Int, false);
+ }
+
+ void StatechartMovePlatformToLandmark::defineSubstates()
+ {
+ RemoteStatePtr stateMoveToLocation = addRemoteState("MovePlatformStateChart","MovePlatformStateChartStateOfferer");
+ StatePtr stateCalcPath = addState<StateCalculatePath>("stateCalculatePath");
+ StatePtr stateSuccess = addState<SuccessState>("stateSuccess");
+ StatePtr stateFailure = addState<FailureState>("stateFailure");
+
+ setInitState(stateCalcPath, ParameterMapping::createMapping()->mapFromParent("*","*"));
+ //transitions
+ addTransition<EvPathFound>(stateCalcPath, stateMoveToLocation, ParameterMapping::createMapping()
+ ->mapFromParent("*","*")
+ ->mapFromOutput("targetPositions")
+ );
+ addTransition<EvNoPathFound>(stateCalcPath, stateFailure);
+ addTransition<EvSuccess>(stateMoveToLocation, stateSuccess);
+ addTransition<EvFailure>(stateMoveToLocation, stateFailure);
+ }
+
+ void StatechartMovePlatformToLandmark::onEnter()
+ {
+ ARMARX_LOG << eVERBOSE << "Entering StatechartMovePlatformToLandmark";
+ }
+
+ void StatechartMovePlatformToLandmark::onExit()
+ {
+ ARMARX_LOG << eVERBOSE << "Exiting StatechartMovePlatformToLandmark...";
+ }
+
+ // ****************************************************************
+ // Implementation of StateCalculatePath
+ // ****************************************************************
+
+ void StateCalculatePath::defineParameters()
+ {
+ addToInput("targetLandmark", VariantType::String, false);
+ addToInput("landmarkNodes", VariantType::List(VariantType::FramedVector3), false);
+ addToInput("landmarkEdges", VariantType::List(VariantType::String), false);
+ addToOutput("targetPositions", VariantType::List(VariantType::Vector3), false);
+ }
+
+ void StateCalculatePath::onEnter()
+ {
+ ARMARX_LOG << eVERBOSE << "Entering StateCalculatePath";
+// std::string targetLandmark = getInput<std::string>("targetLandmark");
+// SingleTypeVariantListPtr landmarkTags = getInput<SingleTypeVariantList>("landmarkTags");
+
+// int targetIndex = -1;
+// for (int i = 0; i < landmarkTags->getSize(); i++) {
+// if (landmarkTags->getVariant(i)->getString() == targetLandmark) {
+// targetIndex = i;
+// break;
+// }
+// }
+
+// SingleTypeVariantList targetPositions(VariantType::Vector3);
+
+// if (targetIndex >= 0) {
+// Vector3Ptr v = getInput<SingleTypeVariantList>("landmarkPositions")->getVariant(targetIndex)->get<Vector3>();
+// Vector3 vv;
+// vv.x = v->x;
+// vv.y = v->y;
+// vv.z = v->z;
+// ARMARX_LOG << eVERBOSE << vv.x << " " << vv.y << " " << vv.z << flush;
+// targetPositions.addVariant(Variant(vv));
+//// targetPositions.addVariant(v);
+
+// sendEvent<EvPathFound>();
+// } else {
+// ARMARX_LOG << eWARN << "Could not map from landmark tag to target position(s)";
+// sendEvent<EvNoPathFound>();
+// }
+
+// setOutput("targetPositions", targetPositions);
+
+ //read the nodes into a tag to node map
+ SingleTypeVariantListPtr landmarkNodes = getInput<SingleTypeVariantList>("landmarkNodes");
+ std::map<std::string, NodePtr> nameToNodeMap;
+ for (int i = 0; i < landmarkNodes->getSize(); i++) {
+ FramedVector3Ptr fv = landmarkNodes->getVariant(i)->get<FramedVector3>();
+ NodePtr n = NodePtr(new Node());
+ n->name = fv->frame;
+ n->framedPos = fv;
+ nameToNodeMap[fv->frame] = n;
+ }
+
+ //process the edges by adding the nodes to each other's successors
+ SingleTypeVariantListPtr landmarkEdges = getInput<SingleTypeVariantList>("landmarkEdges");
+ for (int i = 0; i < landmarkEdges->getSize(); i++) {
+ std::string edge = landmarkEdges->getVariant(i)->getString();
+ std::vector<std::string> edgeNodes;
+ boost::split(edgeNodes, edge, boost::is_any_of("-"), boost::token_compress_on);
+ NodePtr left = nameToNodeMap[edgeNodes[0]];
+ NodePtr right = nameToNodeMap[edgeNodes[1]];
+ left->successors.push_back(right);
+ right->successors.push_back(left);
+ }
+
+ PlatformContext* context = getContext<PlatformContext>();
+ ChannelRefPtr poseRef = context->getChannelRef(context->getPlatformUnitObserverName(), "platformPose");
+ const float platformPositionX = context->getDatafieldRef(poseRef, "positionX")->getDataField()->getFloat();
+ const float platformPositionY = context->getDatafieldRef(poseRef, "positionY")->getDataField()->getFloat();
+
+ //has trouble resolving the observer?
+// const float platformPositionX = context->getDataFromObserver(DataFieldIdentifierPtr(new DataFieldIdentifier("platformPose.positionX")))->getFloat();
+// const float platformPositionY = context->getDataFromObserver(DataFieldIdentifierPtr(new DataFieldIdentifier("platformPose.positionY")))->getFloat();
+
+ std::map<std::string, NodePtr>::iterator it = nameToNodeMap.begin();
+ if (it == nameToNodeMap.end()) {
+ ARMARX_LOG << eWARN << "No nodes provided";
+ sendEvent<EvNoPathFound>();
+ return;
+ }
+ NodePtr closest = it->second;
+ float minDist = getDist(platformPositionX, platformPositionY, closest->framedPos->x, closest->framedPos->y);
+ it++;
+ for (; it != nameToNodeMap.end(); it++) {
+ const float dist = getDist(platformPositionX, platformPositionY, it->second->framedPos->x, it->second->framedPos->y);
+ if (dist < minDist) {
+ closest = it->second;
+ minDist = dist;
+ }
+ }
+
+ ARMARX_LOG << eVERBOSE << "Starting point: " << closest->name << flush;
+
+ std::string landmark = getInput<std::string>("targetLandmark");
+ NodePtr goal = nameToNodeMap[landmark];
+ std::list<NodePtr> path = aStar(closest, goal);
+ SingleTypeVariantList targetPositions(VariantType::Vector3);
+ if (path.empty()) {
+ ARMARX_LOG << eWARN << "No path found";
+ sendEvent<EvNoPathFound>();
+ } else {
+ const float goalAlpha = path.back()->framedPos->z;
+ for (std::list<NodePtr>::iterator it = path.begin(); it != path.end(); it++) {
+ Vector3 v;
+ v.x = (*it)->framedPos->x;
+ v.y = (*it)->framedPos->y;
+ v.z = goalAlpha;
+ targetPositions.addVariant(v);
+ }
+ sendEvent<EvPathFound>();
+ setOutput("targetPositions", targetPositions);
+ }
+ ARMARX_LOG << eVERBOSE << "StateCalculatePath: Done onEnter()" << flush;
+ }
+
+ void StateCalculatePath::onExit()
+ {
+ ARMARX_LOG << eVERBOSE << "StateCalculatePath: Done onExit()";
+ }
+
+ //http://en.wikipedia.org/wiki/A*_search_algorithm
+ std::list<StateCalculatePath::NodePtr> StateCalculatePath::aStar(StateCalculatePath::NodePtr start, StateCalculatePath::NodePtr goal) {
+ std::list<NodePtr> path;
+
+ std::vector<NodePtr> closedSet;
+ std::vector<NodePtr> openSet;
+ openSet.push_back(start);
+ std::map<NodePtr, float> gScore;
+ gScore[start] = 0;
+ std::map<NodePtr, float> fScore;
+ fScore[start] = gScore[start] + heuristic(start, goal);
+ std::map<NodePtr, NodePtr> cameFrom;
+ cameFrom[goal] = start; //in case start==goal
+
+ while (!openSet.empty()) {
+ for (size_t i = 0; i < openSet.size(); i++) {
+ std::cout << openSet[i] << ";";
+ }
+ std::cout << std::endl;
+ float lowestScore = fScore[openSet[0]];
+ std::vector<NodePtr>::iterator currentIT = openSet.begin();
+ for (std::vector<NodePtr>::iterator it = openSet.begin() + 1; it != openSet.end(); it++) {
+ if (fScore[*it] < lowestScore) {
+ lowestScore = fScore[*it];
+ currentIT = it;
+ }
+ }
+ NodePtr current = *currentIT;
+ if (current == goal) {
+ NodePtr cameFromNode = goal;
+ while (cameFromNode != start) {
+ path.push_front(cameFromNode);
+ cameFromNode = cameFrom[cameFromNode];
+ }
+ path.push_front(start);
+ break;
+ }
+ openSet.erase(currentIT);
+ closedSet.push_back(current);
+
+ for (size_t i = 0; i < current->successors.size(); i++) {
+ NodePtr neighbor = current->successors[i];
+ if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end()) continue;
+
+ float tentativeGScore = gScore[current] + heuristic(current, neighbor);
+ bool notInOS = std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end();
+ if (notInOS || tentativeGScore < gScore[neighbor]) {
+ cameFrom[neighbor] = current;
+ gScore[neighbor] = tentativeGScore;
+ fScore[neighbor] = tentativeGScore + heuristic(neighbor, goal);
+ if (notInOS) {
+ openSet.push_back(neighbor);
+ }
+ }
+ }
+ }
+ return path;
+ }
+}
+
diff --git a/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.h b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d0cae21270c6e552dcaee58d3c78d2c09cb4079
--- /dev/null
+++ b/source/RobotAPI/statecharts/MovePlatformToLandmark/MovePlatformToLandmark.h
@@ -0,0 +1,102 @@
+/**
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* 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 Lesser 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::MovePlatformToLandmark
+* @author Valerij Wittenbeck
+* @date 2014 Valerij Wittenbeck
+* @copyright http://www.gnu.org/licenses/gpl.txt
+* GNU General Public License
+*/
+
+#ifndef ARMARX_COMPONENT_MOVE_PLATFORM_TO_LANDMARK_H
+#define ARMARX_COMPONENT_MOVE_PLATFORM_TO_LANDMARK_H
+
+#include <Core/statechart/Statechart.h>
+#include <RobotAPI/statecharts/MovePlatform/PlatformContext.h>
+
+/*
+* This statechart is meant to bridge the gap between a user verbally specifying a target location (i.e. fridge, table, ...)
+* and the robot knowing which points it needs to go to in order to reach that landmark.
+*
+* Ideally, there is some planning involved (graph retrieving/generating, path searching, etc), but this basic form only associates
+* a landmark tag with a series of points that need to be traversed through without respect to the robot's current position.
+*/
+
+namespace armarx
+{
+ class MovePlatformToLandmarkStateChart : public RemoteStateOfferer<PlatformContext>
+ {
+ public:
+ void onInitRemoteStateOfferer();
+ void onConnectRemoteStateOfferer();
+ std::string getStateOffererName() const { return "MovePlatformToLandmarkStateChart"; }
+ };
+
+ // ****************************************************************
+ // Events
+ // ****************************************************************
+
+ DEFINEEVENT(EvPathFound)
+ DEFINEEVENT(EvNoPathFound)
+
+ // ****************************************************************
+ // Definition of StatechartMovePlatformToLandmark
+ // ****************************************************************
+ struct StatechartMovePlatformToLandmark :
+ StateTemplate<StatechartMovePlatformToLandmark>
+ {
+ void defineParameters();
+ void defineSubstates();
+ void onEnter();
+ void onExit();
+ };
+
+ // ****************************************************************
+ // Definition of StateCalculatePath
+ // ****************************************************************
+ /**
+ * StateCalculatePath: Calculate set of points for the robot to drive through
+ */
+
+ struct StateCalculatePath :
+ StateTemplate<StateCalculatePath>
+ {
+ struct Node {
+ std::string name;
+ std::vector<boost::shared_ptr<Node> > successors;
+ FramedVector3Ptr framedPos;
+ };
+ typedef boost::shared_ptr<Node> NodePtr;
+
+ void defineParameters();
+ void onEnter();
+ void onExit();
+
+ inline float getDist(float x1, float y1, float x2, float y2) {
+ const float dX = x1 - x2;
+ const float dY = y1 - y2;
+ return std::sqrt(dX * dX + dY * dY);
+ }
+
+ std::list<NodePtr> aStar(NodePtr start, NodePtr goal);
+ inline float heuristic(NodePtr n1, NodePtr n2) {
+ return getDist(n1->framedPos->x, n1->framedPos->y, n2->framedPos->x, n2->framedPos->y);
+ }
+ };
+
+}
+
+#endif